CN202622807U - 基于GSK-Link总线的模块化机器人控制装置 - Google Patents

Abstract

本实用新型公开了一种基于GSK-Link总线的模块化机器人控制装置，装置包括用于完成机器人示教、示教检查、再现运动控制、示教编程、程序解释、预处理及参数设置的主控制器；对机器人运动数据进行正逆解算法、插补算法、速度控制、轨迹规划的算法控制器；完成对GSK-Link总线控制的GSK-Link总线板；显示程序编辑和机器人状态的显示屏；及用来实现操作输入和灯指示显示按键板，所述主控制器分别和显示屏、算法控制器以及GSK-Link总线板连接，所述GSK-Link总线板与按键板连接。本实用新型通过GSK-Link总线能快速响应请求，高速传输数据，实时检测状态，提高了系统实时性和安全性。另外，本实用新型各组成部分功能模块化，添加或修改单个模块不会影响其他模块的功能，提高了系统的可靠性。

Description

基于GSK-Link总线的模块化机器人控制装置

技术领域

本实用新型涉及工业控制的技术领域，特别涉及基于GSK-Link总线的模块化机器人控制装置。

背景技术

工业机器人是高新技术密集的机电一体化产品。它是提高生产过程自动化，改善劳动环境条件，提高产品质量和生产效率的一种非常有效的手段，也是技术革命的重要内容之一。机器人控制装置作为机器人运动控制的核心装置，对它的设计要求也不断提高，为了使机器人控制更加灵活高效，目前越来越多的系统采用模块化，具有总线功能的运动控制装置。但是现在大多数机器人控制装置采用PCI、CAN、DeviceNet总线传输，PCI是并行总线无法连接太多设备，总线扩展性比较差，线间干扰将导致系统无法正常工作；CAN和DeviceNet总线的传输速率最大只有1Mbits/s，不能满足大数据量传输需要，总线上最多只能挂接110个设备。而基于PCI总线上位机大多仍采用普通计算机，体积大、成本较高。

实用新型内容

本实用新型的目的在于克服现有技术的缺点与不足，提供一种基于GSK-Link总线的模块化机器人控制装置。

为了达到上述目的，本实用新型采用以下技术方案：

本实用新型一种基于GSK-Link总线的模块化机器人控制装置，包括用于完成机器人示教、示教检查、再现运动控制、示教编程、程序解释、预处理及参数设置的主控制器；对机器人运动数据进行正逆解算法、插补算法、速度控制、轨迹规划的算法控制器；完成对GSK-Link总线控制的GSK-Link总线板；显示程序编辑和机器人状态的显示屏；及用来实现操作输入和灯指示显示按键板，所述主控制器分别和显示屏、算法控制器以及GSK-Link总线板连接，所述GSK-Link总线板与按键板连接。

优选的，所述主控制器包括中央处理芯片、显示接口、键盘接口及通信接口，所述中央处理芯片运行嵌入式linux系统，构成控制系统的上位机；所述显示接口由LVDS连接到显示屏；所述键盘接口由GSK-Link总线板的共享内存传到中央处理芯片。

优选的，所述上位机包括调度模块、示教控制模块、示教检查控制模块、再现控制模块、解释模块、预处理模块、通信模块、监控故障处理模块、参数设置模块以及显示模块，所述调度模块用于在启动机器人系统后，建立一个正常的工作环境，启动调度线程、通信线程、解释线程及界面线程，然后等待按键操作并监控当前机器人的状态，协调上位机的各个模块完成机器人控制；

示教控制模块通过操作轴操作键选择坐标系及速度等级来完成机器人示教运动，达到用户指定的位置；

示教检查控制模块用来检查示教好的程序是否按预想的路径或逻辑运行，通过按下“前进”或“后退”键进行逐行检查，直到达到预想的编程要求；

再现控制模块用于实现机器人的自动运行，机器人读取示教检查好的程序，根据程序的指令逐行自动运行；

解释模块由词法分析、语法分析、语义分析及符号表组成，它的作用是提取程序文件里的指令、数据、符号信息；

预处理模块用于对机器人的运动数据进行路径规划，使机器人按规划路径运行；

通信模块是指与算法控制器的通信和与GSK-Link总线板的通信；

监控故障处理模块由监控模块和故障处理模块组成，所述监控模块用于机器人状态监控、IO监控、位姿监控及变量监控，所述故障处理模块用于伺服报警错误、轨迹规划错误、通信错误及总线连接错误现象的处理；

参数设置模块用于运动参数设置、伺服参数设置、运用参数设置、轴最大速度及加速度设置、软极限设置及干涉区设置；

显示模块用于显示机器人状态、示教编程、参数设置和界面监控，为使用者显示一个友好操作界面，所述显示模块使用minigui，minigui是一款面向嵌入式系统的高级窗口系统(Windowing System)和图形用户界面(Graphical UserInterface，GUI)支持系统。

优选的，所述算法控制器由DSP芯片和通信接口组成；所述DSP芯片包括初始化模块、运动控制模块、参数设置模块以及状态参数更新模块；所述通信接口包括与主控制器通信的HPI及与GSK-Link总线板通信的共享内存。

优选的，所述初始化模块用于对FPGA的初始化、DSP内部初始化以及机器人相关参数的初始化；

运动控制模块是算法控制器的核心模块，包括手动模式和自动模式的运动控制方式；完成机器人运动数据的正逆解算法、插补算法、轨迹规划；

参数设置模块接收上位机的轴参数设置和笛卡尔参数设置，通过算法使用轴参数和笛卡尔参数来规划机器人运动，包括各轴最大速度、加速度、加速时间、停机减速度参数；

状态参数更新模块用来更新全局状态参数、轴状态参数、笛卡尔状态参数。

优选的，所述GSK-Link总线板包括FPGA芯片和GSK-Link总线接口，所述FPGA芯片以每2ms时间间隔从DSP芯片读取一次插补数据，FPGA芯片内部再作一个六关节的数据同步处理，通过GSK-Link总线发送到机器人的六个伺服控制装置，实现机器人六轴联动控制；所述GSK-Link总线是一个基于工业以太网的总线，通信周期为微秒级，使总线数据达到快速传输和精准同步；通过GSK-Link总线还具有设置伺服参数，监控伺服状态，连接IO装置功能。

本实用新型相对于现有技术具有如下的优点及效果：

1、本实用新型采用的GSK-Link总线是一种环形网络拓扑结构，GSK-Link通信协议规范是一种数字化、串行网络的数据总线，通信速率最高达到100Mbits/s，挂接设备最多达255个，电气传输采用平衡方式。

2、本实用新型GSK-Link总线具有精准同步机制，通过总线连接机器人六个关节伺服装置，实现同步控制。本实用新型采用高性能ARM核作为一个嵌入式上位机系统，采用DSP作为运动控制算法核，各个CPU通过高速接口和共享内存方式进行通信，如图1所示。

3、本实用新型由多模块组成，具有较高的稳定性、很好的移植性、体积小、成本低、实时控制性强的特点。

4、本实用新型由多个芯片组成，各个芯片根据各自性能完成相应功能提高资源利用率，减少了处理的复杂度，增强了可靠性。

5、本实用新型显示模块使用minigui，具有较好的人机界面，采用LVDS连接控制器和显示屏具有较好的显示抗干扰性。

附图说明

图1为本实用新型机器人控制装置的结构框图；

图2为本实用新型操作调度流程图；

图3为本实用新型示教控制流程；

图4为本实用新型示教检查控制流程图；

图5为本实用新型再现控制流程图；

图6为本实用新型解释总体流程图；

图7为本实用新型词法分析器图；

图8为本实用新型语法分析器图；

图9为本实用新型语义分析图；

图10为本实用新型预处理流程图；

图11为本实用新型DSP总体流程图；

图12为本实用新型手动模式运动控制流程图；

图13为本实用新型自动模式运动控制流程图；

图14为本实用新型总体控制流程图；

图15为本实用新型暂停处理流程图；

图16为本实用新型加速状态时暂停处理效果图；

图17为本实用新型匀速状态时暂停处理效果图；

图18为本实用新型减速状态时暂停处理效果图。

具体实施方式

下面结合实施例及附图对本实用新型作进一步详细的描述，但本实用新型的实施方式不限于此。

实施例

本实施例一种基于GSK-Link总线的模块化机器人控制装置，如图1所示，包括主控制器、算法控制器、GSK-Link总线板、显示屏、按键板；所述主控制器主要完成机器人示教、示教检查、再现运动控制、示教编程、程序解释、预处理、参数设置功能；所述算法控制器主要对机器人运动数据进行正逆解算法、插补算法、速度控制、轨迹规划运算；所述GSK-Link总线板作为GSK-Link总线控制器，主要完成对GSK-Link总线的控制；它把接收到的运动数据、参数数据发送到伺服装置，并把伺服状态数据和GSK-Link总线的状态数据传到上位机进行相应的控制；所述显示屏用来显示程序编辑和机器人状态状况，便于操作者使用。按键板用来实现操作输入和灯指示显示。

主控制器与算法控制器通过HPI接口实现高速通信，发送运动控制指令和运动数据到算法控制器。算法控制器的状态通过HPI反馈传送到主控制器。主控制器与GSK-Link总线板通过共享内存的方式，实现数据高速传输控制，进行伺服参数设置、伺服状态监控、按键板数据接收。算法控制器通过共享内存方式把运算后的轨迹数据传送到GSK-Link总线板实现机器人运动。

所述的主控制器是一个高性能ARM核芯片，运行嵌入式linux系统，构成控制系统的上位机。上位机有调度模块、示教控制模块、示教检查控制模块、再现控制模块、解释模块、预处理模块、通信模块、监控故障处理模块、参数设置模块、显示模块。下面就主要模块功能流程作详细说明。

调度模块不能独立运行，必须同控制装置的其他模块一起完成操作。调度模块在控制装置中与其它模块的关系如图2操作调度流程所示，作为各个模块的中间接口，实现各个模块之间变量及状态传递，实现协调控制。其他的各个模块在不同线程中，调度模块通过信号量、互斥量实现变量的保护或模块之间的同步操作，使控制流程按设计的要求完成。如在示教运动过程中不能实现示教检查操作，不然机器人就会出现运动轨迹错误。如机器人在报警状态下是不能运动的，否则会出现预想不到的危险。

示教控制模块是对工业机器人进行示教，以使工业机器人按需要移动到指定位置。示教键由12个键组成，分别为六个关节的正负方向。示教可以选择使用关节坐标、直角坐标、工具坐标或用户坐标来操作，使机器人移动到指定位置，可选择微动、低速、中速或高速示教速度运行。编程时可以获得当前机器人的位姿值，以作为机器人运动的轨迹参考点。图3为示教控制的流程图，它首先等待示教键的输入，如果有示教键按下，则下一步检查机器人当前的状态，判定当前的工作模式是否为示教模式、有没有示教检查在操作、是否有报警或急停、是否在极限或干涉区、外部信号是否异常等情况，如果条件都能满足示教要求，则获取示教需要的参数信息，如速度、坐标系、按键轴方向等，示教控制模块再调用HPI接口函数把示教相关信息发送到算法控制器，由算法控制器生成运动数据，使机器人进行示教运动。

示教检查控制模块在示教状态下，接收到示教检查键(前进键、后退键)时，就开始对打开的示教文件进行示教内容检查。在示教检查中，可以选择示教检查速度(微动、低速、中速、高速)和检查方式(连续、单步)，完成示教检查。如果检查中出现不理想的情况，则可以改变示教点、检查速度、插补方式来达到理想的运动轨迹。在检查过程中，单步时接收上一步或下一步信息后，按当前所在程序行向前或向后检查。如果连续检查，则直接从头到尾检查示教程序。检查完毕后，保存理想的示教程序，以备工作再现时调用。图4是示教检查控制流程图，它首先等待示教检查键的输入，如果有示教检查键按下，再检查机器人当前的状态，除进行上述示教控制中状态检查外，还需进行文件是否打开、文件是否有语法错误检查。如果状态条件都能满足示教检查的要求，则判断按下的示教检查键是“前进”还是“后退”键，如果是前进键则要判断单步检查还是连续检查，而后退键只有单步检查。接着判断是示教检查开始还是暂停重启，如果是示教检查开始，则初始化相关示教检查参数，获取检查速度，把当前模式自动转换示教检查模式，调用解释模块解释好的指令代码，判断当前检查行指令是否为运动指令，是运动指令则调用预处理模块对运动数据进行路径规划，然后把规划好的数据调用HPI接口函数发送到算法控制器，由算法控制器再对数据作进一步处理，非运动指令则调用解释模块对指令进行相应的逻辑运算、跳转等处理。如果是示教检查暂停，一样要判断暂停行是运动指令还是非运动指令，是运动指令则要调用预处理的暂停重启处理，再把处理好的数据调用HPI接口函数发送到算法控制器，而非运动指令暂停则主要是延时的暂停，在暂停过程中延时不会计数，暂停重启则恢复计数。这样示教检查开始检查程序，弹起示教检查键则暂停当前运行，重新按下则继续运行，直到检查结束。在示教检查过程中可以随时修改程序，如点数据、速度、过渡条件、逻辑等，直到符合设计要求。

再现控制模块主要实现机器人的自动运行。在程序示教检查并达到工作设计要求后，就可以进行工作再现了。在正常工作再现中，依次通过示教模式开关选择再现模式，操作选择示教再现程序、设置再现条件(低速启动、限速运行、低速运行或机械锁定)、设置连续或单步再现。伺服准备灯点亮，按下启动键，直接读取并逐行执行当前打开的程序。控制再现的停止有两种情况：异常中止程序和结束程序的运行中止程序。异常中止程序，把示教模式开关选择示教模式，关掉伺服准备，或者有错误报警都会出现异常停止。结束程序的运行中止程序，只需设置再现条件为单步，程序执行完后自动停止。具体程序工作流程如图5所示。机器人在再现模式下时，等待启动键输入，如果有启动键按下，则要检查机器人当前的状态，这个和示教检查控制检查的状态差不多，如果状态条件都满足再现运动的要求，则判断当前机器人状态是再现暂停状态还是再现起始状态。如果是再现起始状态，则初始化再现起始参数，设置为再现状态，如果是再现暂停状态，则恢复暂停参数，获取当前再现行，调用解释器模块解释当前行的指令代码，判断当前行指令是运动指令还是非运动指令，是运动指令则调用预处理模块对运动数据进行路径规划，然后把规划好的数据调用HPI接口函数发到算法控制器，由算法控制器再对数据作进一步处理，非运动指令则调用解释模块对指令进行相应的逻辑运算、跳转等处理。等待当前行处理完成，如果一行处理完成，则自动处理下一行，直到程序最后一行完成，程序再现结束。如果在连续再现方式下，程序自动跳到第一行继续运行。

解释模块的作用主要是提取程序文件里的指令、数据、符号等信息。它主要由词法分析、语法分析、语义分析、符号表组成，其总体流程如图6所示。下面分别对各组成块作一个说明。

词法分析的主要功能是扫描输入串中的各个字符，并从中识别出一些具有独立意义的基本语法单位-单词(正则表达式提供记号的词法结构)。它为语法分析模块提供一个接口，它直接进行文件读取操作，从文件中读取字符流，并以字符为单位，进行分析，然后通过正则表达式提供的词法结构，按照一定的结合法则，把相邻的有联系的字符组成一个记号。因此，设计中主要考虑的就是通过一定的正则表达式把记号的词法结构通过程序描述出来。由于单个字符读入之后直接就可以生成记号，接下来考虑的主要就是词数大于一个字符的记号的考虑，本模块中主要有数字(NUM)、字符串(ID)、等号(EQ)这三种情况是大于一个字符的，以后可能还有各种比较运算符会添加到本模块中。然后用START、INNUM、INID、INEQ、DONE，来标记当前状态，进行后续字符的读取，最后生成一个记号。由于关键字由单独的记号表示，如果是字符串的话，还要判断是否是一个关键字。具体流程如图7所示。

语法分析阶段的任务是“组词成句”，把TOKEN串按语法规则构成更大的语单位，并识别出其中的语法错误。涉及到“组词成句”，当然少不了文法描述。下面给出的是机器人语言的EBNF文法描述。通过此文法，语法分析模块根据所得到的关键字，对其后语句或者单词进行相应的匹配。具体看流程图8所示。

机器人语言的EBNF文法描述

语义分析器完成类型检查，主要是操作数的合法性检查，和成对使用的关键字的匹配性检查。把语义分析单独运行于语法分析之后的主要原因是它可以完成语法分析不能完成的功能，如操作数合法性检查、关键字匹配等。这是因为语法分析是按行检查或者是按语句检查，在一定的语法基础上，它不能实现跨行的无关联关键字匹配检查，这就可以交给语义分析来执行。语义分析器是通过头结点完成对语法树的遍历，并向指定的文本文件输出错误信息。具体如流程图9所示。

预处理模块作用根据程序的运动数据，对机器人的运动数据进行路径规划，使机器人按规划路径运行。如示教检查控制模块和再现控制模块所描述的，在需要执行运动指令时则需要调用预处理模块来完成数据的处理。如图10所示预处理是对运动指令进行处理，包括MOVJ、MOVL、MOVC指令。如果是单独MOVJ、MOVL、MOVC指令则直接存储对应指令数据，它主要包括运动目标点数据，期望速度数据。如果是三种指令的排列组合或相同指令连在一起处理，则要根据指令的过渡条件进行相关计算来规划过渡路径，在经过计算后存储预处理好的数据。等待示教检查模块或再现模块进行调用。

所述算法控制器由DSP芯片、HPI通信接口和共享内存组成。DSP芯片有DSP初始化模块、运动控制模块、参数设置模块、状态参数更新模块，具体如图11所示。DSP主要功能是对运动数据进行正逆解算法、插补算法、速度控制、轨迹规划运算。运动控制模块是算法控制器的核心模块，它分为手动模式控制和自动模式控制，下面对两个模式分别说明。

手动模式控制如图12所示，它主要是手动示教的运动控制方式，在主控制器通过HPI把示教控制数据传送到算法控制器后，手动模式控制首先判断示教是需要在哪一个坐标系下运动，如果是关节空间下的示教，则要调用关节空间下的插补算法，算得离散的关节角度值，通过共享内存方式把关节角度数据传送到GSK-Link总线板。如果是直角坐标系、工具坐标系或用户坐标系，则需要经过相应的插补算法得到离散的位姿点数据，通过逆解函数运算出对应的关节角度数据，通过共享内存方式把数据传送到GSK-Link总线板。如果插补出现问题或逆解不能运算等问题，算法控制器通过共享内存方式把相关状态量发送给主控制器，再进行处理。

自动模式控制如图13所示，它主要是处理在示教检查和再现运动控制方式下的运动数据，在主控制器通过HPI把再现运动数据传送到算法控制器后，自动模式控制首先判断再现运动指令，如果是关节运动，则需要进行关节插补，首先作关节插补预规划，再作关节插补运算，获得离散的关节角度，再对离散的关节角度进行精插补，这样所得数据就可以通过共享内存传送到GSK-Link总线板，实现关节指令的运动。而直线指令和圆弧指令，因为示教点是关节角度数据，则要经过正解函数运算获得位姿数据，再进行直线插补的规划和计算，圆弧插补的规划和计算，获得离散的位姿数据。对离散的位姿点数据再进行逆解运算，得到离散的关节角度数据，对离散的关节角度数据再进行精插补，把精插补数据发到GSK-Link总线板就可以控制机器人作相应的直线运动和圆弧运动。

所述的GSK-Link总线板由FPGA芯片和GSK-Link总线接口组成。FPGA芯片以每2ms时间间隔从DSP芯片读取一次插补数据，FPGA芯片内部再作一个六关节的数据同步处理，通过GSK-Link总线发送到机器人的六个伺服控制装置，实现机器人六轴联动控制。GSK-Link总线是一个基于工业以太网的总线，通信周期可以达到微秒级，使总线数据达到快速传输和精准同步。通过GSK-Link总线还具有设置伺服参数，监控伺服状态，连接IO装置等功能。主控制器可对伺服装置设置参数，它通过共享内存方式，把伺服参数数据通过FPGA芯片传送到GSK-Link总线，由总线传送到各个伺服装置，进行参数设置。主控制器还可以通过总线监控伺服状态，如速度、电流、位置等相关信息和编码器值等，并对伺服的报警状态实时监控，增加控制的安全性。GSK-Link总线还可以连接IO装置，对IO的输入输出状态进行实时监控，并能够改变输出IO的状态，实现对外部信号的控制。

图14为基于GSK-Link总线的模块化机器人的控制方法的流程图，需要模式开关输入装置和按键输入装置。开关输入装置用来切换机器人的工作模式，机器人再现启动和暂停，机器人的急停操作。按键输入用来控制机器人的示教运动，示教检查运动，还有示教编程的界面操作。

机器人控制装置上电后，主控制器开始初始化内部参数，并对算法控制器的相关参数进行初始化，然后检测GSK-Link总线是否工作正常，如果正常则通过共享内存发送伺服参数到GSK-Link总线板，GSK-Link总线板通过GSK-Link总线发送伺服参数到六个伺服装置，机器人控制系统初始化完成。

模式开关在示教模式下只能进行示教，示教检查，示教编程和设置操作。在再现模式下，只能再现操作，机器人的再现启动和暂停开关才有效。机器人在急停或报警状态下任何运动操作都无效。

机器人在正常情况下，在示教模式下示教键按下，主控制器把示教数据通过HPI发送到算法控制器，算法控制器根据示教数据进行相关运算，得到机器人运动数据。算法控制器通过共享内存把运动数据传到GSK-Link总线板。GSK-Link总线板经过同步处理通过GSK-Link总线把运动数据传到伺服装置实现机器人的运动。在这过程中主控制器会通过HPI实时监控算法控制器的状态，通过共享内存实时监控GSK-Link总线和伺服装置的状态，实现对机器人各种状态的及时响应处理。其中示教运动的具体步骤如下：

S11、有示教按键输入；

S12、检查当前是不是示教模式，机器人有没有报警、急停异常状态；

S13、获取示教需要的参数信息，如速度、坐标系、按键轴方向；

S14、通过HPI发送示教数据到算法控制器；

S15、算法控制器对示教数据信息进行计算处理，得到关节数据，通过共享内存传到GSK-Link总线板；

S16、GSK-Link总线板获得机器人运动的关节数据，经过六关节的同步处理得到同步运动数据，通过GSK-Link总线发到伺服装置实现电机转动。

机器人在正常情况下，示教检查键按下或再现模式启动键按下，都需要调用示教文件进行解释处理，非运动指令由主控制器处理，运动指令先进行预处理操作，处理后的数据作为再现数据。其中示教检查包括下述步骤：

S21、有示教检查键按下；

S22、检查当前是不是示教模式，有没有示教文件，机器人有没有报警、急停异常状态；

S23、主控制器调用示教文件进行解释处理，如果有错误要重新编辑；

S24、非运动指令由主控制器处理，主控制器执行完当前指令后，根据是单步检查还是连续检查选择是否执行下一行程序；

S25、运动指令经过预处理提取再现数据，通过HPI发送再现数据到算法控制器；单步检查只能发送一个运动指令，连续检查可以发送连续的运动指令；

S26、算法控制器从HPI读取到再现数据，对再现数据进行相关运算，得到机器人能够执行S3中，节数据，通过共享内存传到GSK-Link总线板；

S27、GSK-Link总线板获得机器人运动的关节数据，经过六关节的同步处理得到同步运动数据，通过GSK-Link总线发到伺服装置实现电机转动；

S28、主控制器同时监控机器人的运动状态，判断发送到算法控制器的再现数据是否执行完，执行完了则执行下一行程序；

S29、循环到S24步，直到程序结束，在这过程通过示教检查键的弹起操作可以暂停当前指令的执行。

步骤S29中，暂停处理包括下述步骤：

S291、主控制器有暂停操作，主控制器暂停运动数据发送，对运动控制相关变量置位。

S292、主控制器发送暂停控制指令到算法控制器，等待算法控制器处理完成。

S293、当算法控制器在运动的过程中接收到暂停指令时需进行停止运动规划，获取暂停指令时的速度V₀、加速度A₀，要求在t_a时间内速度、加速度减为零，如果当前段剩余距离不够减速需要从内存中读取下一段指令信息继续停止运动，当停止结束时，自动清除停止运动标志位，并清空FIFO中的运动数据。

S294、当主控制器获取到算法控制器暂停处理完状态，暂停处理完成。

再现运动包括下述具体步骤：

S31、有再现启动操作；

S32、检查当前是不是再现模式，有没有示教文件，机器人有没有报警，急停异常状态；

S33、初始化再现参数，判断当前是再现暂停重启还是再现启动，如果是再现重启则直接跳到S35步；

S34、主控制器调用示教文件进行解释处理，如果有错误要重新编辑；

S35、非运动指令主控制器处理，处理完后接着执行程序文件下一行；

S36、运动指令经过预处理提取再现数据，通过HPI发送再现数据到算法控制器，连续的运动指令可以多个发送下去；

S37、算法控制器从HPI读取到再现数据，对再现数据进行相关运算，得到机器人能够执行关节数据，通过共享内存传到GSK-Link总线板；

S38、GSK-Link总线板获得机器人运动的关节数据，经过六关节的同步处理得到同步运动数据，通过GSK-Link总线发到伺服装置实现电机转动；

S39、主控制器同时监控机器人的运动状态，当前运动指令是否执行完，完成后接着执行程序的下一行；

S310、循环到S35步，如果是单步方式再现运行到最后一行程序结束，是连续方式再现在执行完最后一行程序后，跳到第一行重新执行。

主控制器通过HPI发送再现数据到算法控制器，算法控制器根据指令进行再现数据运算，得到机器人能够运动的关节数据，算法控制器通过共享内存把运动数据传到GSK-Link总线板。GSK-Link总线板经过同步处理通过GSK-Link总线把运动数据传到伺服装置实现机器人的运动。在这过程中主控制器会通过HPI实时监控算法控制器的状态，通过共享内存实时监控GSK-Link总线和伺服装置的状态，实现对机器人各种状态的及时响应处理。

如图15所示，算法控制器再现暂停过程具体步骤如下：

S41、接到请求停止的信号，机器人停止运动规划；

S42、进行停止运动插补计算；

S43、判断当前段结束速度是否为0，若不为0，则读取下一段运动数据，循环停止运动规划；若为0，则停止运动结束；所述停止运动插补计算的计算公式如下： $\left\{\begin{array}{c}{A}_0+\frac{J_1{t}_a}{2}+\frac{J_2{t}_a}{2}=0\\ V_0+A_0{t}_a+\frac{3J_1{t}_a^2}{8}+\frac{J_2{t}_a^2}{8}=0\end{array}\right.\⇒\left\{\begin{array}{c}{J}_1=-\frac{{4V}_0}{t_a^2}-\frac{{3A}_0}{t_a}\\ J_2=-\frac{{4V}_0}{t_a^2}+\frac{A_0}{t_a}\end{array}\right.$ 其中：V₀为停止指令时其速度，A₀为加速度，t_a为停止时间，J₁，J₂为停止加速度。

假设V₀＝123.238301，A₀＝354.910619，t_a＝0.25s(此时机器人处于加速运动过程中)停止运动时的速度、加速度波形效果如图16所示；V₀＝130.763885，A₀＝0，t_a＝0.25s(此时机器人处于匀速运动过程中)停止运动时的速度、加速度波形效果如图17所示；V₀＝107.635393，A₀＝-622.189317，t_a＝0.25s(此时机器人处于减速运动过程中)停止运动时的速度、加速度波形效果如图18所示。

上述实施例为本实用新型较佳的实施方式，但本实用新型的实施方式并不受上述实施例的限制，其他的任何未背离本实用新型的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本实用新型的保护范围之内。

Claims (4)

1.一种基于GSK-Link总线的模块化机器人控制装置，其特征在于，包括用于完成机器人示教、示教检查、再现运动控制、示教编程、程序解释、预处理及参数设置的主控制器；对机器人运动数据进行正逆解算法、插补算法、速度控制及轨迹规划的算法控制器；完成对GSK-Link总线控制的GSK-Link总线板；显示程序编辑和机器人状态的显示屏；及用来实现操作输入和灯指示显示按键板，所述主控制器分别和显示屏、算法控制器以及GSK-Link总线板连接，所述GSK-Link总线板与按键板连接。

2.根据权利要求1所述基于GSK-Link总线的模块化机器人控制装置，其特征在于，所述主控制器包括中央处理芯片、显示接口、键盘接口及通信接口，所述中央处理芯片运行嵌入式linux系统，构成控制系统的上位机；所述显示接口由LVDS连接到显示屏；所述键盘接口由GSK-Link总线板的共享内存传到中央处理芯片。

3.根据权利要求1所述基于GSK-Link总线的模块化机器人控制装置，其特征在于，所述算法控制器由DSP芯片和通信接口组成；所述通信接口包括与主控制器通信的HPI及与GSK-Link总线板通信的共享内存。

4.根据权利要求1所述基于GSK-Link总线的模块化机器人控制装置，其特征在于，所述GSK-Link总线板包括FPGA芯片和GSK-Link总线接口，所述FPGA芯片以每2ms时间间隔从DSP芯片读取一次插补数据，FPGA芯片内部再作一个六关节的数据同步处理，通过GSK-Link总线发送到机器人的六个伺服控制装置，实现机器人六轴联动控制；所述GSK-Link总线是一个基于工业以太网的总线，通信周期为微秒级，使总线数据达到快速传输和精准同步；通过GSK-Link总线还具有设置伺服参数，监控伺服状态，连接IO装置功能。 

Images