CN110480658B - 一种融合视觉自标定的六轴机器人控制系统 - Google Patents

Abstract

本发明涉及一种融合视觉自标定的六轴机器人控制系统，包括：控制器模块、人机交互模块和机器人驱动模块，所述控制系统还包括：视觉采集模块，用于获取六轴机器人的实际位置和方向信息；关节动作采集模块，用于获取六轴机器人关节的实际动作信息；视觉自标定子模块，用于完成机器人运动学参数和动力学参数的标定；所述视觉采集模块和关节动作采集模块均与控制器模块通信连接，所述视觉自标定子模块集成在控制器模块上，所述视觉自标定子模块基于视觉采集模块和关节动作采集模块获取的信息完成标定。与现有技术相比，本发明具有参数标定易操作，控制准确性高、安全可靠、保护性强、可扩展性高和可维护性好等优点。

Description

一种融合视觉自标定的六轴机器人控制系统

技术领域

本发明涉及机械臂控制技术领域，尤其是涉及一种融合视觉自标定的六轴机器人控制系统。

背景技术

工业串联六轴机器人是一类极其重要的工业机器人，能够代替人类完成大量复杂、危险的加工生产任务。目前，我国的工业机器人尤其是六轴机器人严重依赖于进口，这严重制约了我国的制造行业及相关产业的发展。国产的六轴机器人与国外六轴机器人的差距不仅仅表现在硬件方面(包括电机、减速机、伺服驱动)，更重要的是在控制系统和控制算法方面的不足。因此，研究发明针对六轴机器人的控制系统，具有很重要的学术价值和应用价值。

随着科学技术的发展，硬件方面的差异在逐步缩小，比如新松、新时达、埃夫特等国内机器人厂商的硬件技术发展很快。同时，软件方面国内机器人厂商也在努力跟上。因此需要设计出一套稳定可靠的控制系统。

由于制造精度和工作损耗等问题，通常机械臂的理论运动参数会和实际值有一定的偏差，在对工作精度有很高要求的环境下，往往不能满足要求，传统的标定方式往往通过激光等复杂的方法进行参数整定，也需要专业的人员进行操作，对于使用者来说极为不友好。

另外在一些对控制要求更高的场合中，需要利用机械臂的动力学模型对机械臂进行更高性能的控制，与运动学参数的标定相仿，传统的动力学参数辨识耗时耗力，极为不友好。

授权公告号为CN104476549B的发明专利公开了一种利用视觉测量对机械臂运动路径补偿的方法，用于视觉引导机械臂的运动。该方法通过视觉测量识别机械臂、航天器、待装位置三者位置与理论模型中理论位置存在差异，补偿机械臂路径规划偏差，达到安全进出狭小空间的控制效果，满足航天器高精度高可靠性的装配需求。

该利用视觉测量对机械臂运动路径补偿的方法存在以下缺点：该对机械臂运动路径补偿的方法通过视觉测量识别机械臂、航天器、待装位置三者位置与理论模型中理论位置存在差异，对位置进行补偿，仅对运动学参数进行标定，不够准确。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种融合视觉自标定的六轴机器人控制系统。

本发明的目的可以通过以下技术方案来实现：

一种融合视觉自标定的六轴机器人控制系统，包括：控制器模块、人机交互模块和机器人驱动模块，所述控制系统还包括：

视觉采集模块，用于获取六轴机器人的实际位置和方向信息；

关节动作采集模块，用于获取六轴机器人关节的实际动作信息；

视觉自标定子模块，用于完成机器人运动学参数和动力学参数的标定；

所述视觉采集模块和关节动作采集模块均与控制器模块通信连接，所述视觉自标定子模块集成在控制器模块上，所述视觉自标定子模块基于视觉采集模块和关节动作采集模块获取的信息完成标定，通过视觉自标定子模块完成参数标定后，再对六轴机器人进行控制运行。

进一步地，所述视觉自标定子模块进行运动学参数的标定包括以下步骤：

S101：获取多个标定点；

S102：向控制器模块发送控制指令，使得机械臂运动到第一个标定点位置，同时根据控制指令中的运动学参数计算并记录为理论位姿；

S103：基于六轴机器人的实际位置和方向信息，利用视觉和手眼关系计算当前的六轴机器人末端位姿，并将其记录为实际位姿；

S104：将理论位姿和实际位姿记录到文件中；

S105：遍历所有标定点；

S106：基于所有标定点的理论位姿和实际位姿，优化更新得到新的运动学参数。

进一步地，所述视觉自标定子模块进行动力学参数的标定包括以下步骤：

S1：读取预先设定的轨迹文件及其机器人动力学模型；

S2：向控制器模块发送控制指令，使得机械臂按照轨迹进行运动，同时通过关节动作采集模块记录运动过程中六轴机器人关节的实际动作信息；

S3：利用机器人动力学模型进行参数辨识，获取理论参数；

S4：根据理论参数与实际动作信息，优化更新得到新的动力学参数并记录到文件中。

进一步地，所述控制器模块分两个进程完成六轴机器人的控制，包括：

主控制进程，用于完成控制算法以及与硬件驱动器组之间的通信，主控制进程独立运行，保证了控制器模块的可靠性；

服务进程，用于轨迹规划；

所述主控制进程与服务进程之间通过内存共享的方式进行通信，解决了双进程同步的问题，将服务进程对主控制进程产生的影响控制在最小范围内，确保主控制进程的安全。

进一步地，所述控制算法包括基于C/C++语言的控制器算法，所述基于C/C++语言的控制器算法封装成库，通过调用库函数，完成控制。

进一步地，所述控制器算法的算法框架包括：

电机单元，用于控制指令与实际动作之间的转化；

运动学单元，用于机器人正逆运动学的计算；

操作方式单元，用于机器人具体运动任务的设置；

所述电机单元包含一个主电机控制子单元，该主电机控制单元聚合有多个子电机控制子单元，有利于让所有子电机控制子单元操作上保持一致(比如所有子电机控制子单元一起上电、下电等)，从而保证了控制算法的安全。

进一步地，所述运动学单元为抽象类继承的形式。作为基类的抽象类机器只定义了一些数据的结构(如关节角、位姿等)和运动学实现接口，至于具体的运动学计算方法则有继承类来实现。这样设计有利于应用上的拓展，例如要使用此框架对SCARA机器人进行控制的话，只需继承一个新的子类，然后实现SCARA机器人相应的正逆运动学函数就可以了。

进一步地，所述控制器模块包括基于TCP协议的通信接口，保证了控制器的高度可扩展性，可以更加方便的完成多机协作及功能扩展。

进一步地，所述人机交互模块包括示教器和客户端，所述示教器通过Modbus协议与控制器模块通信，所述客户端通过TCP协议完成与示教器相同的任务，增加了整个系统的操作方式，更加可靠。

进一步地，所述基于Modbus协议的示教器通信数据与基于TCP协议的客户端通信数据均被控制器模块转化为统一的抽象命令结构。这样设计的原因，一方面是为了将不同通信方式的具体通信过程、通信数据与控制器的具体操作解耦，确保代码的安全性，另一方面是为了能够将不同通信方式最终导致的控制器具体操作一致化(调用一样的函数)，减少代码的冗余同时，也增强了代码的可维护性。

进一步地，所述控制系统的控制方法包括以下步骤：

1)系统初始化；

1.1)人机交互模块获取是否重新标定当前运动学参数指令，若是执行步骤1.2)，若否，执行步骤2)；

1.2)人机交互模块依次进行运动学参数的标定和动力学参数的标定；

2)系统基于预存储的控制文件或人机交互模块接收的实时控制指令完成对应的控制。

与现有技术相比，本发明具有以下优点：

(1)本发明六轴机器人控制系统引入了视觉自标定子模块，完成机器人运动学参数和动力学参数的标定，并能实现一键标定，提高了六轴机器人控制系统的控制准确性且更加方便，解决了传统机械臂控制系统中参数标定复杂且难以操作的问题。

(2)本发明中控制器模块分两个进程完成六轴机器人的控制，包括：主控制进程和服务进程，主控制进程独立运行，完成控制算法，保证了控制器模块的可靠性。

(3)本发明采用内存共享的方式，使得主控制进程与服务进程同步，将服务进程对主控制进程产生的影响控制在最小范围内，确保主控制进程的安全。

(4)本发明采用C/C++语言挖完成控制器算法，并将其封装成库，完成控制，使得控制器算法能实现更复杂的算法且对源码的保护性更强。

(5)本发明控制器模块包括基于TCP协议的通信接口，保证了控制器的高度可扩展性，可以更加方便的完成多机协作及功能扩展。

(6)本发明人机交互模块包括示教器和客户端，增加了整个系统的操作方式，更加可靠；示教器和客户端与控制器模块不同协议类型的通信数据均被转化为统一的抽象命令结构，确保代码的安全性，减少代码的冗余同时，也增强了代码的可维护性。

附图说明

图1为本发明融合视觉自标定的六轴机器人控制系统的结构示意图；

图2为本发明视觉自标定子模块进行运动学参数标定的进程示意图；

图3为本发明视觉自标定子模块进行动力学参数标定的进程示意图；

图4为本发明工控机端基于CoDeSys的主控制进程的算法部署示意图；

图5为本发明主控制进程的UML类图关系；

图6为本发明共享内存的数据结构示意图；

图7为本发明人机交互协议的UML类图关系。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例1

如图1所示，本实施例为一种融合视觉自标定的六轴机器人控制系统，包括控制器模块1、人机交互模块2、机器人驱动模块3、视觉采集模块、关节动作采集模块和视觉自标定子模块。

控制器模块1分别与人机交互模块2和机器人驱动模块3通信连接。视觉采集模块和关节动作采集模块均与控制器模块1通信连接，视觉自标定子模块集成在控制器模块1上，视觉自标定子模块基于视觉采集模块和关节动作采集模块获取的信息完成标定。

下面对上述一些模块进行详细描述：

1、视觉自标定子模块

如图2所示，视觉自标定子模块进行运动学参数的标定包括以下步骤：

S101：获取多个标定点，运动学参数的标定需要采集多个理论位姿和实际位姿，因此需要获取多个标定点；

S102：向控制器模块1发送控制指令，使得机械臂运动到第一个标定点位置，同时根据控制指令中的运动学参数计算并记录为理论位姿；

S103：基于六轴机器人的实际位置和方向信息，利用视觉和手眼关系计算当前的六轴机器人末端位姿，并将其记录为实际位姿；

S104：将理论位姿和实际位姿记录到文件中；

S105：遍历所有标定点；

S106：基于所有标定点的理论位姿和实际位姿，优化更新得到新的运动学参数。

当系统启动时，在交互界面选择是否重新标定当前运动学参数，若需要重新标定，则系统进入运动学参数和动力学参数的标定进程。

如图3所示，视觉自标定子模块进行动力学参数的标定包括以下步骤：

S1：读取预先设定的轨迹文件及其机器人动力学模型；

S2：向控制器模块1发送控制指令，使得机械臂按照轨迹进行运动，同时通过关节动作采集模块记录运动过程中六轴机器人关节的实际动作信息；

S3：利用机器人动力学模型进行参数辨识，获取理论参数；

S4：根据理论参数与实际动作信息，优化更新得到新的动力学参数并记录到文件中。

在交互界面选择是否标定动力学参数，若进行标定，则进入动力学参数的标定进程。标定完成后，整个系统初始化完成。

2、关节动作采集模块

本实施例中关节动作采集模块为多个机械臂传感器。

3、机器人驱动模块3

机器人驱动模块3包括驱动器组301和机器人本体上的电机组302。

4、控制器模块1

控制器模块1分两个进程完成六轴机器人的控制，包括：主控制进程，为CoDeSys(PLC软件编程工具)进程，用于完成控制算法以及与硬件驱动器组301之间的通信；服务进程，用于轨迹规划；主控制进程与服务进程之间通过共享内存的方式进行通信。

控制器模块1设置在工控机主站端，工控机端基于CoDeSys的主控制进程算法部署方案具体为：因为既要完成与驱动器组301之间的可靠通信，又要高效设计实现机器人的控制算法，因此采用了CoDeSys作为一个中间桥梁，使用C++完成核心算法的编写，将核心控制代码编译成库，CoDeSys读取驱动器组301的传感器数据，然后将这些数据作为函数参数传递给底层的核心控制代码，完成控制器模块1对机器人的控制。

如图4所示，CoDeSys进程扮演着EtherCAT主站的角色，负责与驱动器收发指令，进而控制机器人本体的运动。CoDeSys有提供直接的IO变量映射功能，编程人员只需将IO变量(如输入变量状态字、编码器读数、输出变量控制字、目标位置等)映射到自己设计代码中的某个程序变量，然后通过改变这些程序变量的值，就能够方便地修改Ethercat主站向从站发送的PDO数据内容，进而控制机器人的运动。

然而，CoDeSys本身只支持IEC61131-3标准的编程语言，即结构化文本语言(ST)、顺序功能进程图语言(SFC)、功能模块语言(FBD)、指令表语言(IL)和梯形图语言(LD)。这些语言的优点是编写容易、开发难度低、代码逻辑能够可视化表示，与之伴随的缺点是难以深度定制实现复杂算法、对源码的保护性弱。目前，PLC语言主要被用来二次开发，完成实际的业务需求(如产品流水线的进程控制)，这对于控制器核心算法的设计而言显然是不合适的，因此，选用C/C++语言来设计控制器核心算法，然后将其封装成库。PLC语言通过调用库函数来完成具体的计算，最后将计算结果映射到IO变量，完成整个控制的进程。

因此，在本实施例中设计好合适的库函数接口，将主体的编程平台由基于CoDeSys的PLC语言转变为C/C++语言。为了让控制器算法源码能够更加完美地封装起来，以确保其安全性，将库函数接口的格式，与EtherCAT主从站的PDO数据格式定义成了一样的格式。换言之，由C/C++编写的控制器算法，能够从这个接口得到PDO的原始输入变量，且算法的最终计算结果，也将直接映射到PDO的输出变量上。

如此，不仅将控制器算法完美封装，同时也将PLC的代码缩减到最简。在进行具体控制任务的定制时，PLC代码只起到一个“传值”的作用(将PDO的映射数据传给库函数)，本身不负责任何的业务逻辑实现。具体的实现是交由C/C++编写的控制器算法和控制终端的通信来完成：控制终端(如示教器201)向控制器发送指令，控制器根据指令，来选择合适的算法计算输出(OUT)变量，进而PLC代码调用库函数的结果变成“动态”的，从而实现了具体任务的定制。

下面对主控制进程的算法设计和共享内存进行具体介绍：

1)如图5所示，主控制进程通过CoDeSys的C++代码库作为实现载体，主控制进程的算法框架可划分为三个模块：电机单元101、运动学单元102和操作方式单元103。电机单元101负责抽象数据与PDO实际数据之间的转化，如关节角度(抽象)与编码器数值(实际)、电机上下电(抽象)与控制字填写(实际)、电机运行状态监控(抽象)与状态字解析等。运动学单元102负责机器人正逆运动学的计算。操作方式单元103负责机器人具体运动任务的设置。

电机单元101包含一个主电机控制子单元(motors)，motors聚合了多个子电机控制子单元(motor)，其中每个motor的参数(位数、减速比等)保存在JSON文件中，当算法初始化的时候，通过读取JSON文件的数据，来具体构造每个motor对象的。与PLC代码的接口函数由motors来实现，这样有利于让所有motor操作上保持一致(比如所有motor一起上电、下电等)，从而保证了控制算法的安全。

运动学单元102设计成了抽象类继承的形式，作为基类的抽象类机器人(Robot)只定义了一些数据的结构(如关节角、位姿等)和运动学实现接口，至于具体的运动学计算方法则有继承类来实现。这样设计有利于应用上的拓展，例如要使用此框架对SCARA机器人进行控制的话，只需从Robot继承一个新的子类，然后实现SCARA相应的正逆运动学函数就可以了。运动学计算中需要用到的参数(如D-H参数)同样保存在JSON文件中，在算法初始化的时候进行数据加载和对象构建。

2)共享内存

轨迹序列由服务进程来生成，生成好以后由主控制进程访问，用以后续的计算。但不同的进程之间，是无法直接进行内存的相互访问的，因此需要主进程和服务进程之间进行通信。进程间通信(IPC)的方式很很多种，包括管道(pipe)、套接字(socket)、共享内存(sharedmemory)等。一般而言，服务进程生成的轨迹序列都是成千上万的，且需要与主进程进行双向的数据流动，因此，选用共享内存的方法最为合适。

两个进程要想共同完成一个任务，就需要同步和通信。用于进程间同步的已有技术包括信号(signal)、信号量(semaphore)等，但这些方法会对整个CoDeSys运行时进程造成影响，一旦处理不当，可能会造成整个进程的崩溃，产生严重后果；再者，CoDeSys运行时进程本身也对信号的处理作了一些设计，倘若复用这些信号来同步轨迹生成的算法进程，也会使程序变得复杂，并埋下安全隐患。因此，本节不采用额外的IPC方式，而是通过设计共享内存的具体数据结构的办法，来解决双进程同步的问题，将服务进程对CoDeSys进程产生的影响控制在最小范围内，确保CoDeSys主进程的安全。

因此在本发明中，通过共享内存完成进程间的通信，利用共享内存中的信号变量来完成进程之间的同步。

如附图6所示，U1用来存储轨迹参数，比如轨迹的几何形状、起点坐标、终点坐标、最大速度、最大加速度、插补时间间隔等。完整的轨迹可能是由多段子轨迹组合而成，因此轨迹参数变量pathParms是一个数组。由于共享内存的创建需要向内核显式声明内存的数据长度，不支持动态变化，因此还需要一个lengthPathParm整型变量来指明的有效长度。

U2用来存储算法的生成结果，即轨迹点的序列。1个轨迹点由3个位置变量和3个姿态变量构成，1条轨迹由多个轨迹点构成，因此轨迹序列是6维向量的数组，同时也需要另一个lengthPath变量来指明序列点的有效个数。

除此之外，U1和U2还各自拥有一个枚举类型的参数(S1和S2)，用来控制主进程和服务进程之间的交互逻辑。

5、人机交互模块2

对于许多已经产品化的机器人，最常见的人机交互的媒介是通过示教器201。用户通过示教器201的图形界面，制定实际应用中的运动任务，这种交互方式非常简便、容易上手，但也存在着缺点，就是难以对任务进行深度定制，图形化的界面会使能供选择的功能、参数受到限制，难以通过界面全部展现出来。

另一种常见的交互方式是将底层功能以库的形式封装成API(ApplicationProgramming Interface)，然后用户通过调用相关的API来设计具体功能，这种方式主要面向的用户群体是程序人员，需要对编程有一定的基础，常用于二次开发过程。这种方法能够有效地满足任务的深度定制，但带来的相应的缺点是难以上手、不够直观，用户需要事先阅读配套的技术文档，了解API各个接口函数的调用规则才能开始编程。另一个缺点是会受到平台或者编程语言的限制。无论是使用哪种编程语言封装成库，或者针对哪个应用平台封装成库，都难以使交互接口广泛适用，用户必须得事先熟悉相关的编程语言或应用平台，才能够开始使用API来编写程序。

为了避免传统API上手困难的问题，本发明设计了一种基于套接字(socket)的协议来完成人机交互的过程，这种方式能够摆脱编程语言和应用平台的限制，方便二次开发人员控制机器人。同时，为了兼顾目前工业领域的控制协议规范，也设计了基于Modbus的控制协议。人机交互模块2设置在从站中，包括两部分，一为示教器201部分，在专用的示教器201上运行，通过Modbus协议完成与主站之间的通信，二为客户端202部分，在工控机上运行，通过TCP协议可完成与示教器201相同的任务，增加了整个系统的操作方式，更加可靠。

如图7所示，TCP通信和Modbus通信分别由TcpServer和ModbusTcpServer类来负责，这两种通信方式虽然有着不同的数据交互过程和不同的数据含义定义，但最终都会将各自的具体接受数据转化为统一的抽象命令结构CommandData，然后由CommandHandler根据CommandData的内容去调用相关函数下发指令(包括前文提到的选择合适的Solver和更新相应的SolverCommand)。这样设计的原因，一方面是为了将不同通信方式的具体通信过程、通信数据与控制器的具体操作解耦，确保代码的安全性，另一方面是为了能够将不同通信方式最终导致的控制器具体操作一致化(调用一样的函数)，减少代码的冗余同时，也增强了代码的可维护性。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (7)

1.一种融合视觉自标定的六轴机器人控制系统，包括：控制器模块、人机交互模块和机器人驱动模块，其特征在于，所述控制系统还包括：

视觉采集模块，用于获取六轴机器人的实际位置和方向信息；

关节动作采集模块，用于获取六轴机器人关节的实际动作信息；

视觉自标定子模块，用于完成机器人运动学参数和动力学参数的标定；

所述视觉采集模块和关节动作采集模块均与控制器模块通信连接，所述视觉自标定子模块集成在控制器模块上，所述视觉自标定子模块基于视觉采集模块和关节动作采集模块获取的信息完成标定，通过视觉自标定子模块完成参数标定后，再对六轴机器人进行控制运行；

所述控制器模块分两个进程完成六轴机器人的控制，包括：

主控制进程，用于完成控制算法以及与硬件驱动器组之间的通信；

服务进程，用于轨迹规划；

所述主控制进程与服务进程之间通过内存共享的方式进行通信；

所述控制算法包括基于C/C++语言的控制器算法，该基于C/C++语言的控制器算法封装成库，所述控制器算法的算法框架包括：

电机单元，用于控制指令与实际动作之间的转化；

运动学单元，用于机器人正逆运动学的计算；

操作方式单元，用于机器人具体运动任务的设置；

所述电机单元包含一个主电机控制子单元，该主电机控制子单元聚合有多个子电机控制子单元。

2.根据权利要求1所述的一种融合视觉自标定的六轴机器人控制系统，其特征在于，所述视觉自标定子模块进行运动学参数的标定包括以下步骤：

S101：获取多个标定点；

S102：向控制器模块发送控制指令，使得机械臂运动到第一个标定点位置，同时根据控制指令中的运动学参数计算并记录为理论位姿；

S103：基于六轴机器人的实际位置和方向信息，利用视觉和手眼关系计算当前的六轴机器人末端位姿，并将其记录为实际位姿；

S104：将理论位姿和实际位姿记录到文件中；

S105：遍历所有标定点；

S106：基于所有标定点的理论位姿和实际位姿，优化更新得到新的运动学参数。

3.根据权利要求1所述的一种融合视觉自标定的六轴机器人控制系统，其特征在于，所述视觉自标定子模块进行动力学参数的标定包括以下步骤：

S1：读取预先设定的轨迹文件及其机器人动力学模型；

S2：向控制器模块发送控制指令，使得机械臂按照轨迹进行运动，同时通过关节动作采集模块记录运动过程中六轴机器人关节的实际动作信息；

S3：利用机器人动力学模型进行参数辨识，获取理论参数；

S4：根据理论参数与实际动作信息，优化更新得到新的动力学参数并记录到文件中。

4.根据权利要求1所述的一种融合视觉自标定的六轴机器人控制系统，其特征在于，所述运动学单元为抽象类继承的形式。

5.根据权利要求1所述的一种融合视觉自标定的六轴机器人控制系统，其特征在于，所述控制器模块包括基于TCP协议的通信接口。

6.根据权利要求1所述的一种融合视觉自标定的六轴机器人控制系统，其特征在于，所述人机交互模块包括示教器和客户端，所述示教器通过Modbus协议与控制器模块通信，所述客户端通过TCP协议完成与示教器相同的任务。

7.根据权利要求6所述的一种融合视觉自标定的六轴机器人控制系统，其特征在于，所述基于Modbus协议的示教器通信数据与基于TCP协议的客户端通信数据均被控制器模块转化为统一的抽象命令结构。

Images