一种用于多机器人的轨迹控制系统与方法与流程

本发明属于多机器人协同控制技术领域，尤其涉及一种用于多机器人的轨迹控制系统与方法。

背景技术：

随着我国在重大结构件制造领域的需求增加，其高精度、高柔性的制造特点对加工装备提出了新的挑战。如直径大于3米、长度大于10米的大型密封舱结构，为保证其功能和精度满足设计指标要求，需整体进行加工，现有机床难以满足加工范围的要求，大型构件的高效高精度制造成为制约我国高端制造业发展的主要瓶颈。

移动式混联加工机器人由全向移动平台、混联加工机器人两个主要部分组成。全向移动平台能够在地面上，托举上面的混联加工机器人围绕工件做平移和旋转运动。混联加工机器人则实现末端铣刀5自由度运动，实现5自由度加工的功能。基于移动式加工机器人的制造模式为实现大型构件的整体加工提供了新的思路。世界各国都有大量科研机构开展了将移动机器人用于大型构件的原位制造研究。

基于可编程控制器plc的多机器人协同控制系统，主要由plc作为上位机控制各台机器人的运动，主要有两种方式，一种是控制机器人的程序号，机器人的运动程序以程序号为标识事先下载到机器人控制器中，上位机plc通过调用机器人控制器中的程序号，使得机器人执行该程序号下的轨迹。另一种方式是控制机器人的目标点位，上位机plc将目标坐标点实时发送给下位机器人控制器，机器人控制器自主完成当前位置到目标点的插补轨迹。

上述方法的问题在于上位控制器无法获知机器人的实际运动轨迹，第一种方式只能预先把运动程序下载到机器人控制器中，如果需要更改机器人的运动轨迹，则需要重新编制并下载新的运动程序，第二种方式虽然完成了机器人运动目标点的控制，但由于无法得知机器人在运动到目标点过程中的状态，运动轨迹精度完全由机器人控制器确定，各台机器人运动轨迹同步性较差，同时插补步长和速率不同容易导致两台机器人在运动过程中发生碰撞。

技术实现要素：

本发明的技术解决问题：克服现有技术的不足，提供一种用于多机器人的轨迹控制系统与方法，用于多台机器人协同控制，与传统的多台机器人控制系统相比，可显著提高机器人的运动灵活性和控制精度，提高机器人控制的柔性自动化水平。

为了解决上述技术问题，本发明公开了一种用于多机器人的轨迹控制系统，包括：

上位控制器，用于根据接收到的数控指令，解算得到转动角度-时间曲线；

总线耦合器，用于将所述转动角度-时间曲线发送至下位机器人控制器；

下位机器人控制器，用于根据接收到的转动角度-时间曲线，完成机器人各关节和末端执行器电机的驱动。

在上述用于多机器人的轨迹控制系统中，上位控制器，包括：

预读子系统，用于预读并解释接收到的数控指令，确定数控指令的类别，将功能指令通过上位机通讯模块传递至上位plc模块，将运动指令通过上位机通讯模块传递至粗插补计算模块；其中，数控指令的类别，包括：功能指令和运动指令；

粗插补计算模块，用于按照系统设定的粗插补时间步长对运动指令进行分段，使运动指令离散化成为设定时间长度内机器人末端法兰移动距离和转角的函数；

伺服计算模块，用于将所述机器人末端法兰移动距离和转角的函数，通过机器人逆运动学算法解算为机器人各个旋转关节对应的电机旋转角度值，得到转动角度-时间曲线；

上位plc模块，用于接收功能指令，并执行所述功能指令所指示的功能；

上位机通讯模块，用于通过统一的编码方式，将功能指令传递至上位plc模块，将运动指令传递至粗插补计算模块，实现串行交互。

在上述用于多机器人的轨迹控制系统中，上位控制器，还包括：用户界面接口，用于接收用户输入的数控指令。

在上述用于多机器人的轨迹控制系统中，功能指令，包括：机器人的启动、停止、运行速度、等待时间长度、各台机器人的同步动作点；运动指令，包括：机器人末端的运动轨迹，并以标准的数控g代码形式表达。

在上述用于多机器人的轨迹控制系统中，机器人末端法兰移动距离和转角的函数包括三个过程描述：末端法兰中心点移动距离、末端法兰法线转角和末端法兰沿初始法线的转角；其中，末端法兰中心点移动距离是指：末端法兰中心点按规划的直线或者圆弧路径和指定速度，从一个起始点到终止点的距离过程描述；末端法兰法线转角是指末端法兰的法线方向按照规划的速度，从初始法线方位运动到终止发现方位的转角过程描述函数；末端法兰沿初始法线的转角是指末端法兰的法线按照规划的速度，从初始位置向终止位置过程中，沿着初始法线转动的角度过程描述函数。

在上述用于多机器人的轨迹控制系统中，转动角度-时间曲线是指：在直角坐标系中以水平轴为时间刻度，竖直轴为电机转动角度。

在上述用于多机器人的轨迹控制系统中，下位机器人控制器，包括：

机器人通讯控制模块，用于下位机器人控制器中各个模块之间的相互通讯，将下位机器人控制器中各个模块之间的交互数据通过统一的编码方式，实现串行交互，通过定义的优先级解决数据通讯阻塞；并对不同的数据按照标准的工业总线协议进行封装，封装后的数据以标准的工业总线协议格式向上位控制器进行传输；

机器人安全plc模块，用于获取通过下位机器人各传感器、编码器采集得到的采集信息，对采集信息进行编码后，使用状态参数传输给上位plc模块；并由上位plc模块将接收到的状态参数与预先保存的参数进行匹配，得到机器人安全plc模块的状态信息，并通过用户界面接口进行显示；

精插补计算模块，用于根据接收到的转动角度-时间曲线，计算关节电机在一个规定的精插补时间段内需要旋转的角度值，得到控制电压-时间曲线；

关节驱动器，用于根据控制电压-时间曲线，完成对各关节驱动器的电机的驱动。

在上述用于多机器人的轨迹控制系统中，状态参数，包括：安全检验信息、驱动器运行信息、驱动器停止信息和上位plc模块与机器人安全plc模块的交互信息。

在上述用于多机器人的轨迹控制系统中，控制电压-时间曲线是指：在直角坐标系中以水平轴为时间刻度，竖直轴为控制电压值，电压值在±12v范围内随时间变化而波动。

相应的，本发明还公开了一种用于多机器人的轨迹控制方法，包括：

通过上位控制器对接收到的数控指令进行解算，得到转动角度-时间曲线；

通过总线耦合器将所述转动角度-时间曲线发送至下位机器人控制器；

由下位机器人控制器根据接收到的转动角度-时间曲线，完成机器人各关节和末端执行器电机的驱动。

本发明具有以下优点：

(1)在本发明中，所有机器人均由上位控制器在线控制运动轨迹。即上位控制器上完成所有下位机器人运动轨迹的规划与实时控制，无需提前将轨迹程序下载至机器人控制器，使得轨迹规划与数控编程过程更加简单，只需编写上位控制器的程序即可。

(2)在本发明中，通过统一的粗插补计算模块实现机器人运动轨迹的协同控制。通过上位控制器统一完成所有下位机器人轨迹的插补运算，保证了各台机器人插补精度的一致性，同时，由于将逆运动学解析模块定义在上位控制器上，上位控制器可以获得各台机器人实时运动位姿，保证了运动的安全性，不会出现轨迹偏差而产生碰撞干涉。

(3)本发明采用粗插补和精插补两步计算法，即能够保证多个机器人的轨迹精度一致性，又能够根据各台机器人的同台特性设置速度、加速度变化规律，尽可能减少上位计算机计算资源的占用。

附图说明

图1是本发明实施例中一种用于多机器人的轨迹控制系统的结构框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公开的实施方式作进一步详细描述。

如图1，在本实施例中，该用于多机器人的轨迹控制系统，包括：

上位控制器101，用于根据接收到的数控指令，解算得到转动角度-时间曲线。

在本实施例中，上位控制器101是整个系统的多台机器人运动轨迹的总控单元，主要可以包括：

用户界面接口1011，用于接收用户输入的数控指令。

在本实施例中，用户界面接口1011是用户输入数控指令的界面，用户将数控指令由键盘、usb或以太网输入上位控制器界面。

预读子系统1012，用于预读并解释接收到的数控指令，确定数控指令的类别，将功能指令通过上位机通讯模块1016传递至上位plc模块1015，将运动指令通过上位机通讯模块1016传递至粗插补计算模块1013。

在本实施例中，预读子系统1012主要由指令预读模块和指令解释模块组成。数控指令首先进入指令预读模块，指令预读条数由系统参数进行设定，为保证运动的连贯性和前瞻性，一般不少于200条。数控指令包括下位所有各台机器人的控制指令，包括：功能指令和运动指令。其中，功能指令包括：机器人的启动、停止、运行速度、等待时间长度、各台机器人的同步动作点；运动指令包括：机器人末端的运动轨迹，并以标准的数控g代码形式表达。指令解释模块负责解释上述功能指令和运动指令，并将功能指令通过上位机通讯模块1016传递至上位plc模块1015，将运动指令通过上位机通讯模块1016传递至粗插补计算模块1013。

粗插补计算模块1013，用于按照系统设定的粗插补时间步长对运动指令进行分段，使运动指令离散化成为设定时间长度内机器人末端法兰移动距离和转角的函数。

在本实施例中，粗插补计算模块1013主要由直线插补器、圆弧插补器和样条插补器组成。机器人末端法兰移动距离和转角的函数包括三个过程描述：末端法兰中心点移动距离、末端法兰法线转角和末端法兰沿初始法线的转角。其中，末端法兰中心点移动距离是指：末端法兰中心点按规划的直线或者圆弧路径和指定速度，从一个起始点到终止点的距离过程描述；末端法兰法线转角是指末端法兰的法线方向按照规划的速度，从初始法线方位运动到终止发现方位的转角过程描述函数；末端法兰沿初始法线的转角是指末端法兰的法线按照规划的速度，从初始位置向终止位置过程中，沿着初始法线转动的角度过程描述函数。

伺服计算模块1014，用于将所述机器人末端法兰移动距离和转角的函数，通过机器人逆运动学算法解算为机器人各个旋转关节对应的电机旋转角度值，得到转动角度-时间曲线。

在本实施例中，伺服计算模块1014主要由标准编译循环模块、用户定义编译循环模块和位置控制模块组成。标准编译循环模块和用户定义编译循环模块主要用于实现逆运动学算法。其中，标准编译循环模块内嵌标准机器人的逆运动学算法和机器人各关节电机减速器的减速比，能够将机器人末端法兰移动距离和转角通过本循环转换为电机的转动角度，构造转动角度-时间曲线。用户定义编译循环模块则内嵌用户自定义的逆运动学算法和末端电机的减速器减速比，包括新增带有平移或旋转功能的末端执行器，能够将末端执行器的平移距离和旋转角度转换为电机的转动角度，构造转动角度-时间曲线。位置控制模块则监测来自上位机通讯模块获得的下位机器人控制器各电机的实时转角，当下位机器人控制器各电机的实时转角与标准编译循环模块和用户定义编译循环模块计算得到的电机转角值不一致时，则会将这两个转角差值通过上位机通讯模块发送给系统总线，下位机器人接收到差值后控制电机旋转相应的角度，直至差值小于某一个设定的微小数值，形成位置闭环控制。

优选的，转动角度-时间曲线是指：在直角坐标系中以水平轴为时间刻度，竖直轴为电机转动角度。在这个直角坐标系下，曲线上任意一点表示当前时间点上电机的绝对角度值，该曲线是连续的，且满足一阶连续条件，即曲线在任一时间点上的曲率(角速度)是连续的。

其中，需要说明的是，逆运动学算法是指公知的机器人dh运动学变换。通过dh方法获得各关节电机旋转的校对值，这里的dh方法可以通过《机器人学导论》2006年机械工业出版社，作者(美)johnj.craig，isbn9787111186816等文献查阅。

上位plc模块1015，用于接收功能指令，并执行所述功能指令所指示的功能。

在本实施例中，上位plc模块1015除了接收功能指令，并执行所述功能指令所指示的功能之外，还可以在每一个plc循环周期内监测上位控制器和下位机器人控制器各通讯端口、行程开关、安全开关、各电机编码器、电机电流、电压、电网电压、各组件温度、安全门、急停按钮的闭合状态，当系统中出现异常情况时，则启动相应的紧急处理程序，如急停、电机缺电抱闸，保证系统的安全运行。

上位机通讯模块1016，用于通过统一的编码方式，将功能指令传递至上位plc模块，将运动指令传递至粗插补计算模块，实现串行交互。

在本实施例中，上位机通讯模块1016主要是将各模块之间的交互数据通过统一的编码方式，实现串行交互，通过定义的优先级解决数据通讯阻塞。并对不同的数据按照标准的工业总线协议进行封装，封装后的数据能够以标准的工业总线协议格式向其他控制器进行传输。

总线耦合器102，用于将所述转动角度-时间曲线发送至下位机器人控制器。

在本实施例中，总线耦合器102主要用于统一并转换上位控制器与下位机器人控制器之间工业总线协议，使得它们之间的通讯稳定可靠。例如上位控制器采用profinet工业总线，通过总线耦合器转换为下位机器人控制器的ethercad工业控制总线。

下位机器人控制器103，用于根据接收到的转动角度-时间曲线，完成机器人各关节和末端执行器电机的驱动。

在本实施例中，下位机器人控制器103是每一台机器人单独的控制单元，除了执行上位控制器的指令外，还负责完成机器人各关节和末端执行器电机的驱动，具体可以包括：

机器人通讯控制模块1031，用于下位机器人控制器中各个模块之间的相互通讯，将下位机器人控制器中各个模块之间的交互数据通过统一的编码方式，实现串行交互，通过定义的优先级解决数据通讯阻塞；并对不同的数据按照标准的工业总线协议进行封装，封装后的数据以标准的工业总线协议格式向上位控制器进行传输。

机器人安全plc模块1032，用于获取通过下位机器人各传感器、编码器采集得到的采集信息，对采集信息进行编码后，使用状态参数传输给上位plc模块；并由上位plc模块将接收到的状态参数与预先保存的参数进行匹配，得到机器人安全plc模块的状态信息，并通过用户界面接口进行显示。

在本实施例中，状态参数可以包括以下三组信息：(1)安全检验信息：下位机器人外部自动化运行启动/停止，安全急停、操作人员防护装置确认、机器人手动/自动运行模式、确认按钮、外围设备确认、外围接触器开关。(2)驱动器运行/停止信息，包括每一个关节驱动器位置控制、速度控制、电流控制数据状态，关节驱动器对应的编码器、光栅尺读取的当前电机位置、电机速度与上位控制器中伺服计算模块中的参数值偏差值。(3)上位plc模块与机器人安全plc模块的交互信息，包括报警、运行状态等需要在用户界面接口显示的信息。

精插补计算模块1033，用于根据接收到的转动角度-时间曲线，计算关节电机在一个规定的精插补时间段内需要旋转的角度值，得到控制电压-时间曲线。

在本实施例中，精插补计算模块1033，计算关节电机在一个规定的精插补时间段内需要旋转角度值，包括：运动学参数设置模块、动力学参数设置模块和多项式插补器三个部分组成。

优选的，运动学参数设置模块设置的主要参数包括：精插补时间步长、所有关节电机的最大速度。动力学参数设置模块设置的主要参数包括：所有关节电机的最大加速度和最大加加速度。多项式插补器是通过运动学参数设置模块和动力学参数设置模块确定的参数值作为多项式插补条件，将伺服计算模块输出的转动角度-时间曲线转换为在插补时间步长上，每一个关节电机所对应的加速、匀速和减速的时间段，以及每一个时间段上起始的速度值、加速度值和加加速度值。

其中，当上位机控制模块的粗插补时间步长与精插补计算模块设定精插补时间步长相等时，将直接采用多项式插补器将输入的转动角度-时间曲线计算得到电机在每个插补时间步长内的加速、匀速和减速的时间段，以及每一个时间段上起始的速度值、加速度值和加加速度值，并转换成为控制电压-时间曲线输出给各关节驱动器。当上位机控制模块的粗插补时间步长是精插补计算模块设定精插补时间步长的倍数时，精插补计算模块首先对输入的电机的转动角度-时间曲线按照该倍数进行细分，然后再在细分的每个时间步长内，采用多项式插补器计算得到电机在每个插补时间步长内的加速、匀速和减速的时间段，以及每一个时间段上起始的速度值、加速度值和加加速度值，并转换成为控制电压-时间曲线输出给各关节驱动器。

优选的，控制电压-时间曲线是指：在直角坐标系中以水平轴为时间刻度，竖直轴为控制电压值，电压值在±12v范围内随时间变化而波动。在这个直角坐标系下，曲线上任意一点表示当前时间点上电机的控制电压值，该曲线是连续的，且满足一阶连续条件，即曲线在任一时间点上的曲率是连续的。

关节驱动器1034，用于根据控制电压-时间曲线，完成对各关节驱动器的电机的驱动。

在本实施例中，关节驱动器的数量可以是多个，通过接收到的控制电压-时间曲线，实现对电机的驱动。

在上述实施例的基础上，本发明还公开了一种用于多机器人的轨迹控制方法，包括：通过上位控制器对接收到的数控指令进行解算，得到转动角度-时间曲线；通过总线耦合器将所述转动角度-时间曲线发送至下位机器人控制器；由下位机器人控制器根据接收到的转动角度-时间曲线，完成机器人各关节和末端执行器电机的驱动。

对于方法实施例而言，由于其与系统实施例相对应，所以描述的比较简单，相关之处参见系统实施例部分的说明即可。

本说明中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。

以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。