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

摘要

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

为了达到上述第二目的，本发明采用以下技术方案：

本发明一种基于GSK-Link总线的模块化机器人的控制方法，包括下述步骤：

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

S2、机器人在正常情况下，在示教模式下示教键按下，主控制器把示教数 据通过HPI发送到算法控制器，算法控制器根据示教数据进行相关运算，得到 机器人运动数据；

S3、算法控制器通过共享内存把运动数据传到GSK-Link总线板，GSK-Link 总线板经过同步处理通过GSK-Link总线把运动数据传到伺服装置实现机器人的 运动；

S4、机器人在正常情况下，示教检查键按下或再现启动键按下，都需要调 用示教文件进行解释处理，非运动指令由主控制器处理，运动指令先进行预处 理操作，处理后的数据作为再现数据；

S5、主控制器通过HPI发送再现数据到算法控制器，算法控制器根据指令 进行再现数据运算，得到机器人能够运动的关节数据，算法控制器通过共享内 存把运动数据传到GSK-Link总线板；

S6、GSK-Link总线板经过同步处理通过GSK-Link总线把运动数据传到伺 服装置实现机器人的运动；在这过程中主控制器会通过HPI实时监控算法控制 器的状态，通过共享内存实时监控GSK-Link总线和伺服装置的状态，实现对机 器人各种状态的及时响应处理。

优选的，步骤S2中，示教运动具体包括下述步骤：

S11、有示教按键输入；

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

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

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

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

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

优选的，步骤S4中，示教检查包括下述步骤：

S21、有示教检查键按下；

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

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

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

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

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

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

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

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

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

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

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

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

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

优选的，步骤S4中，再现运动包括下述步骤：

S31、有再现启动操作；

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

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

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

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

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

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

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

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

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

优选的，算法控制器再现暂停过程具体步骤如下：

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

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

S43、判断当前段结束速度是否为0，若不为0，则读取下一段运动数据， 循环停止运动规划；若为0，则停止运动结束；所述停止运动插补计算的计算公 式如下：

$\left(\begin{array}{c}{A}_0+\frac{J_1{t}_2}{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)\Rightarrow \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₂为停止加速 度。

本发明相对于现有技术具有如下的优点及效果：

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

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

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

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

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

6、本发明采用独特的暂停程序处理算法，具有流程简洁、计算高效的特点。 假设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所示。

附图说明

图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}_2}{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)\Rightarrow \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所 示。

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