CN113064708B - 高速连续运动中的机械臂系统多任务协同与信息同步方法 - Google Patents

Abstract

本发明涉及一种高速连续运动中的机械臂系统多任务协同与信息同步方法，包括机械臂硬件子系统和机械臂软件子系统；机械臂软件子系统包括机械臂操作系统、机械臂应用程序集，和数据存储中心；机械臂应用程序集的传感器数据读取模块从通信数据缓冲区中读取传感数据，若获取到有效数据则将当前时刻记为传感数据获取时间全局值。机械臂工作数据快速获取模块快速计算获取与机械臂实际工作任务直接关联的机械臂工作数据。数据同步匹配模块处理传感数据子步骤将传感数据和机械臂工作数据进行同步匹配。机械臂应用程序集和外部监控系统共同实现了机械臂本体装置在高速运动条件下的工作状态获取、外部感知、数据同步匹配、外部监控多种类型任务间的协同。

Description

高速连续运动中的机械臂系统多任务协同与信息同步方法

技术领域

本发明涉及一种机械臂多任务间的协同和信息同步方法。更明确地，本发明涉及一种高速连续运动中的机械臂系统多任务协同与信息同步方法。

背景技术

机械臂尤其是工业机械臂装置已经在制造领域取得了广泛的应用，对于搬运、码垛等精度要求不高的任务，其相关的技术已基本发展成熟。近年来，人们已开始尝试将机械臂应用于电子制造领域中的精密测试、精密装配等具有高精度、高速度特点的任务，也因而产生了一系列新的技术问题。

上述精密操作任务既需要机械臂进行持续、高速地运动，以满足严苛的任务时间需求，也需要机械臂能够实时地对任务场景进行有效感知，进而实现如同产线工人般的“手眼协调”、“快速盲操”。另一方面，尽管任务的难度和复杂度越来越高，但商用工业机器人产品目前依然普遍以自身的可靠性和成本为主要研制目标，使得产品的计算能力、存储空间、通信处理能力等资源较为有限。因此从技术应用推广的角度，基于工业机器人产品的技术方案必须充分考虑实际技术限制。

对于连续高速运动的机械臂，其自身的运动状态数据、操作的执行状态数据、外部传感器获取的感知数据的获取频率也需要随之提高，才能保证数据间的间隔符合高精度的要求。然而，单位时间内数据密度的增加也随之提升了在数据间进行匹配的难度。在这种背景下，如何基于有限的软硬件资源，实现对于上述多种数据间的有效获取和匹配，成为亟待解决的技术问题。

发明内容

本发明的目的在于面向具有高精度、高速度特点的精密测试、精密装配等需求，考虑当前机械臂产品的技术制约，实现一种适用于高速连续运动中的机械臂系统的任务执行和信息同步方法。

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

机械臂系统包含机械臂硬件子系统和机械臂软件子系统。机械臂硬件子系统包含机械臂本体装置、机械臂控制器和外部传感器装置；机械臂控制器包含通信总线接口；外部传感器装置与机械臂控制器间通过通信总线接口建立通信连接，外部传感器装置能够高速周期化地感知采样并发送传感数据，其感知采样与传感数据发送频率由用户预先设定；外部传感器装置的具体形式包括空间多维力传感器，空间多维力传感器安装于机械臂本体装置的末端。

机械臂软件子系统运行于机械臂控制器之上；机械臂软件子系统包含机械臂操作系统、机械臂应用程序集，和数据存储中心；通信总线接口由机械臂操作系统负责管理，机械臂操作系统包含通信数据缓冲区，该缓冲区用于高速地存储从通信总线接口中获得的传感数据SensorData；机械臂应用程序集包含传感器数据读取模块、机械臂工作数据快速获取模块、数据同步匹配模块；传感器数据读取模块周期化地从通信数据缓冲区中读取传感数据SensorData，若获取到有效数据则将当前时刻记为传感数据获取时间全局值nG_ClockSync_F，并将其存储至同步数据缓冲区，同步数据缓冲区包括机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、同步时间子缓冲区nVClockBuf；机械臂工作数据快速获取模块用于快速计算获取与机械臂实际工作任务直接关联的机械臂工作数据WorkData，若获取到有效数据则将当前时刻记为工作数据获取时间值ClockSync_W；数据同步匹配模块用于将传感数据SensorData和机械臂工作数据WorkData进行同步匹配。

机械臂应用程序集中的模块具有不同的实时性需求，随之对应于不同的程序运行方式；

机械臂工作数据快速获取模块嵌入于机械臂的主任务程序的循环体中连续执行，从而高速地获取机械臂工作数据，使得具有高速连续特征的机械臂运动任务得以准确执行和实时监控；主任务程序的循环体紧随轨迹运动指令之后，其循环结束的条件为轨迹运动指令对应的实际动作执行完毕。

将传感器数据读取模块每次被调用激活并开始运行时的系统全局时间记为T_{f_global}；传感器数据读取模块的单次预计执行时长记为T_{f_predict}，该模块每隔时长T_f被激活执行，按需设定T_f使得T_{f_predict}<T_f；若该模块的单次实际执行时长T_{f_actual}超过了T_f，则当其运行时长等于T_f时，该模块不会被重复激活执行，而是直至本次运行结束，在此情况下，该模块下次被激活执行的时刻为T_{f_global}+(n+1)*T_f，n为整数且有n*T_f<T_{f_actual}<(n+1)*T_f。

传感器数据读取模块每隔时长T_f被激活执行的具体方式包括：通过机械臂操作系统中包含的时钟周期化激活执行，和通过其它周期化触发的条件激活执行。

数据存储中心，存储的数据包括：

传感数据SensorData、机械臂工作数据WorkData；

机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、同步时间子缓冲区nVClockBuf，这三组子缓冲区的容量值均为长度值nG_BufLength，数据结构均为数组型，数组元素索引均从0开始；

传感数据同步完成位置索引值nG_BufsaveIdx，用于标识已完成同步处理的缓冲区段中的最末端索引位置；

传感数据写入位置索引值nVBufSaveIdx，用于标识供最新获得的机械臂工作数据WorkData执行写入操作的缓冲区索引位置；

待同步缓冲区段长度值nVbuf，用于标识等待进行同步匹配处理的缓冲区段实际长度；

传感数据旧值SensorDataOld，

传感数据获取时间全局值nG_ClockSync_F，

传感数据获取时间本地新值ClockSync_F，

传感数据获取时间本地旧值ClockSync_F_old，

传感数据获取时间本地初值ClockSync_F_ini，

传感数据写入位置偏移值nBias_Vbuf，

传感数据写入位置偏移总量值nBiasN_Vbuf，

工作数据获取时间值ClockSync_W。

数据同步匹配模块，其运行过程包括：

S1：数据同步匹配初始化子步骤；

S2：存储机械臂工作数据子步骤；

S3：处理传感数据子步骤，该子步骤进一步分为S3-1、S3-2、S3-3、S3-4四个细分步骤；

S3-1：判断最新获得的传感数据是否是新数据，若是则继续，若非则返回；

S3-2：依据传感数据获取时间ClockSync_F计算传感数据写入位置偏移总量值nBiasN_Vbuf；

S3-3：依据传感数据写入位置偏移总量值nBiasN_Vbuf，计算和存储传感数据子缓冲区nG_SensorDataBuf中相关区段的数值；

S3-4：数据同步匹配更新操作。

数据同步匹配初始化子步骤S1仅在初始运行阶段执行1次；存储机械臂工作数据子步骤S2和处理传感数据子步骤S3组成程序组，嵌入于机械臂的主任务程序的循环体中连续执行，并位于机械臂工作数据快速获取模块之后。

数据同步匹配初始化子步骤S1，其具体方法为：

将所述传感数据写入位置索引值nVBufSaveIdx、所述传感数据同步完成位置索引值nG_BufSaveIdx、所述待同步缓冲区段长度值nVbuf的值均置为0，

将传感数据获取时间本地初值ClockSync_F_ini的值赋给所述传感数据获取时间本地旧值ClockSync_F_old；

存储机械臂工作数据子步骤S2，其具体方法为：

将所述传感数据写入位置索引值nVBufSaveIdx的值自加1后，再对所述nG_BufLength进行取余运算，将得到的结果赋值给nVBufSaveIdx，

将所述待同步缓冲区段长度值nVbuf的值自加1，

将所述机械臂工作数据WorkData存储至所述机械臂工作数据子缓冲区nG_WorkDataBuf数组中的第nVBufSaveIdx项，

将所述工作数据获取时间值ClockSync_W存储至所述同步时间子缓冲区nVClockBuf数组中的第nVBufSaveIdx项。

机械臂工作数据WorkData包含机械臂本体装置的末端姿态，以及与该末端姿态相对应的局部操作姿态。

机械臂工作数据快速获取模块的工作方式具体为：

每一次执行机械臂本体装置的局部操作运动轨迹时，设置与该轨迹运动指令相对应的轨迹编号，记录为nMoveID；nMoveID的值随轨迹的数量而同步递增，其编号由机械臂操作系统管理并负责生成；在机械臂本体装置按照该轨迹运行过程中，通过机械臂操作系统实时获取机械臂本体装置的轨迹执行数据nMoveID_Now，得到该运动轨迹的执行比例为nMoveID_Now-nMoveID，计算获得机械臂工作数据WorkData为：

WorkData＝nMoveID起始位置对应的工作数据+(nMoveID_Now-nMoveID)*nMoveID对应的完整区间数据。

机械臂本体装置是否正在按照该轨迹运行的判断条件为：

轨迹编号nMoveID不为空值，且有nMoveID_Now≤nMoveID+1；

nMoveID对应的完整区间数据，为与该轨迹所对应的局部操作姿态的数值区间长度。

处理传感数据子步骤S3，其细分步骤S3-1的具体方法为：

先令ClockSync_F＝nG_ClockSync_F，再判断ClockSync_F与ClockSync_F_old是否相等，若不相等，则传感数据SensorData是新数据，若相等则不是新数据。

处理传感数据子步骤S3，其细分步骤S3-2的具体方法为：

先令nBiasN_Vbuf＝0，再对如下条件进行判断，若该条件满足，则nBiasN_Vbuf的值自加1，直至该条件不满足：

ClockSync_F≥nVClockBuf[(nG_BufsaveIdx+nBiasN_Vbuf)％nG_BufLength]且nBiasN_Vbuf<nVbuf。

处理传感数据子步骤S3，其细分步骤S3-3的具体方法为：

若有ClockSync_F_old等于ClockSync_F_ini，则处理如下：若nBiasN_Vbuf等于1，则将传感数据SensorData的值直接存储至nG_SensorDataBuf[nG_BufSaveIdx]，若nBiasN_Vbuf不等于1，则对于nBias_Vbuf∈[0,nBiasN_Vbuf-1]，按如下插值方式计算获得nG_SensorDataBuf数组中索引位置为nIdxA的值：

nG_SensorDataBuf[nIdxA]＝SensorData*nBias_Vbuf/(nBiasN_Vbuf-1)，其中，nIdxA的值为(nG_BufSaveIdx+nBias_Vbuf)％nG_BufLength。

若ClockSync_F_old不等于ClockSync_F_ini，则对nBias_Vbuf∈[0,nBiasN_Vbuf-1]，按如下插值方式计算获得nG_SensorDataBuf数组中索引位置为nIdxA的值：

nG_SensorDataBuf[nIdxA]＝SensorDataOld+nCompA*(nVClockBuf[nIdxA]-ClockSync_F_old)，其中，nIdxA的值为(nG_BufSaveIdx+nBias_Vbuf)％nG_BufLength，nCompA＝(SensorData-SensorDataOld)/(ClockSync_F-ClockSync_F_old)；

处理传感数据子步骤S3，其细分步骤S3-4的具体方法为：依次令

将所述传感数据SensorData的值赋给所述传感数据旧值SensorDataOld，

将所述传感数据获取时间本地新值ClockSync_F的值赋给所述传感数据获取时间本地旧值，

将所述传感数据同步完成位置索引值nG_BufSaveIdx与所述传感数据写入位置偏移总量值nBiasN_Vbuf进行求和，再对所述长度值nG_BufLength进行取余计算，将得到的结果赋值给所述待同步缓冲区段长度值nVBuf。

传感数据SensorData由外部传感器装置感知获得，在单次采样及进行常规数据处理后，即通过通信总线接口发送至机械臂操作系统中的通信数据缓冲区；若外部传感器装置的具体形式为空间多维力传感器，其测量维数为n，则传感数据SensorData为由n个值组成的向量；若通信数据缓冲区发生数据溢出，则优先删除其中最早的数据。

传感数据获取时间全局值nG_ClockSync_F的写入操作仅在传感器数据读取模块中被执行1次，其读取操作仅在数据同步匹配模块中被执行1次，以最大程度地提升互斥保护。

数据存储中心的数据用于在外部监控系统中进行保存、显示与后处理。

机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、传感数据同步完成位置索引值nG_BufsaveIdx、长度值nG_BufLength，用于在外部监控系统中进行信息同步后的显示及后处理。

传感数据SensorData、机械臂工作数据WorkData的值，用于在外部监控系统中进行高速实时地显示。

外部监控系统与机械臂控制器间通过通信总线接口建立通信连接，以访问相关数据，机械臂端的相关通信任务由机械臂操作系统进行管理，通信数据传输由外部监控系统主动发起。

机械臂工作数据快速获取模块的设计机制，使得其快速实时获取机械臂工作数据WorkData，即优先对于高速连续运动条件下的相关运动数据进行获取。

传感器数据读取模块的设计机制，使得在小于设定机械臂系统的硬件资源和软件资源占用量的前提下，将传感数据SensorData的读取延迟降低到设定的时间范围内。

机械臂工作数据WorkData的计算获取频率，高于传感数据SensorData的采样获取频率，数据同步匹配模块用于实现这两种数据间的同步。

外部监控系统的任务，低频率、批量化地获取同步数据缓冲区中的数据，从而使得对于机械臂系统的资源占用量小于设定值。

机械臂应用程序集和外部监控系统共同用于实现机械臂本体装置在高速运动条件下的工作状态获取、外部感知、数据同步匹配、外部监控多种类型任务间的协同。

综上，本发明具有如下技术特点及有益效果：

(1)针对多类型数据高速传输的特点，以略微降低时间基准的等距性为代价，实现了以机械臂工作数据的获取作为最优先的任务，保证了机械臂运动轨迹与工作轨迹的实时精密监控；进一步，以工作数据的实际获取时间为基准进行数据同步匹配，获得了以工作数据为自变量、传感数据为因变量的匹配函数，适合于精密测试、精密装配中的运动主导型任务。

(2)提供了工作数据快速获取、传感数据获取时间互斥操作等多种方法，以达到进一步减小计算量、降低系统资源占用的目的。

(3)数据同步匹配操作实时进行，不需后处理阶段进行，适合应用于高速度、高精度、高性能的机械臂应用场景。

(4)占用资源少，不过度依赖于机械臂操作系统和特定的外部传感器装置等，使得本发明适合于在普通商用工业机器人系统中推广应用。

附图说明

图1为本发明所述的机械臂系统的软硬件总体示意图；其中图1(a)所示为所述机械臂硬件子系统在笔记本电脑转轴测试场景中的应用示意图，图1(b)所示为所述机械臂硬件子系统在电子制造生产线中精密装配工序场景中的应用示意图。

图2为本发明所述的传感器数据读取模块的周期化运行示意图。

图3为本发明所述的同步数据缓冲区和相关参数的示意图。

图4为本发明所述的数据同步匹配模块的运行流程图。

图5为本发明所述的机械臂工作数据WorkData及局部操作姿态示意图。

图6为本发明所述的处理传感数据子步骤S3中的细分步骤S3-2的示意图。

图7为本发明所述的外部监控系统及其显示功能示意图。

具体实施方式

本发明的优选实施例结合附图详述如下，但以下实施例仅是说明性的，本发明的保护范围并不受这些实施例的限制。

如图1所示，整个机械臂系统包括机械臂硬件子系统和机械臂软件子系统两部分。其中机械臂硬件子系统包含机械臂本体装置、机械臂控制器和外部传感器装置；机械臂控制器包含通信总线接口；外部传感器装置通过通信总线接口与机械臂控制器建立通信连接，外部传感器装置能够高速周期化地感知采样并发送传感数据，其感知采样与传感数据发送频率由用户预先设定，例如将其设定为每秒钟采样100次，每秒钟通过通信总线接口向外发送100次数据；外部传感器装置的具体形式包括空间多维力传感器，空间力多维传感器安装于机械臂本体装置的末端。图1(a)所示为机械臂硬件子系统在笔记本电脑转轴测试场景中的应用示意图，图1(b)所示为机械臂硬件子系统在电子制造生产线中精密装配工序场景中的应用示意图，其中，外部传感器装置的具体形式均为空间6维力传感器。

机械臂软件子系统运行于机械臂控制器之上，包含机械臂操作系统、机械臂应用程序集和数据存储中心。通信总线接口由机械臂操作系统负责管理，机械臂操作系统包含通信数据缓冲区，该缓冲区用于高速地存储从通信总线接口中获得的传感数据SensorData，存储的最高速率不低于300Hz；机械臂应用程序集包含传感器数据读取模块、机械臂工作数据快速获取模块、数据同步匹配模块；传感器数据读取模块周期化地从通信数据缓冲区中读取传感数据SensorData，若获取到有效数据则将当前时刻记为传感数据获取时间全局值nG_ClockSync_F，并将其存储至同步数据缓冲区，同步数据缓冲区包括机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、同步时间子缓冲区nVClockBuf；机械臂工作数据快速获取模块用于快速计算获取与机械臂实际工作任务直接关联的机械臂工作数据WorkData，单次的计算获取时间不超过1毫秒，若获取到有效数据则将当前时刻记为工作数据获取时间值ClockSync_W，若获取到有效数据则将当前时刻记为工作数据获取时间值ClockSync_W；数据同步匹配模块用于将传感数据SensorData和机械臂工作数据WorkData进行同步匹配。

以图1(a)所示的笔记本电脑转轴测试应用为例，机械臂工作数据WorkData在该应用中即可设定为电脑显示屏转轴的旋转角度，也即电脑显示屏沿轴向的旋转角度，传感数据SensorData为空间6维力传感器获得的力与扭矩值。虽然显示屏由位于机械臂本体装置末端的机械工具推动，随机械臂本体装置末端的运动而运动，例如以每秒300度角往复高速旋转，但显示屏的旋转角度在一般情况下却无法由机械臂本体装置的姿态直接获得，而是需要额外地进行一系列矩阵变换计算才能获得，例如由已知的机械臂本体装置的末端位姿(通常是固定于法兰中心的工具基准坐标系相对于全局坐标系的位姿)，无法直接读取到显示屏的旋转角度信息。除非电脑显示屏的转轴与机械臂本体装置全局坐标系的某坐标轴重合，显示屏的旋转角度才能由机械臂本体装置的姿态通过简单折算后获得。任何的矩阵运算都意味着需要占用系统的计算资源，并需要花费一定的时间，这对于高速运动状态下的机械臂系统来说是需要尽量避免的；在该场景中，电脑显示屏转轴的旋转角度值被存储于机械臂工作数据子缓冲区nG_WorkDataBuf中，与其对应的工作数据获取时间值被存储于同步时间子缓冲区nVClockBuf中，由6维力传感器获得的力与扭矩值被存储于传感数据子缓冲区nG_SensorDataBuf中；数据同步匹配模块用于将空间6维力传感器的读数和电脑显示屏转轴的旋转角度进行同步匹配，以精确获得对应于某个旋转角度值的相关力数据。

在图1(b)所示的电子制造生产线中精密装配工序场景中，由于精密拼装工序往往并非一步按压拼装到位，而是需要预紧、按压、调整，因此机械臂工作数据WorkData可对应于工序的完成比例，而非直接对应于机械臂本体装置的姿态；在该场景中，数据同步匹配模块用于将空间6维力传感器的读数和工序的实时进度进行同步匹配，以精确获得整个工序期间的装配力曲线。

如图2所示，将传感器数据读取模块每次被调用激活并开始运行时的系统全局时间记为T_{f_global}；传感器数据读取模块的单次预计执行时长记为T_{f_predict}，该模块每隔时长T_f被激活执行，合理设定T_f使得T_{f_predict}<T_f；于图示T_{f_global}时刻再次被激活启动的该模块，该次单次实际执行时长T_{f_actual}超过了T_f，但不超过2倍的T_f,则当其运行时长等于T_f时，该模块不会被重复激活执行，而是直至本次运行结束，在此情况下，该模块下次被激活执行的时刻为T_{f_global}+2*T_f。

传感器数据读取模块每隔时长T_f被激活执行的具体方式包括：通过所述机械臂操作系统中包含的时钟周期化激活执行，和通过其它周期化触发的条件激活执行。例如在针对图1(a)所示的笔记本电脑转轴测试应用实例中，可以在显示屏背面固定安装可以检测空间姿态变化的陀螺仪组件，并以陀螺仪某姿态角每隔0.01度的变化作为触发条件，来激活执行传感器数据读取模块。

如图1(a)所示，数据存储中心，存储的数据包括：

传感数据SensorData、机械臂工作数据WorkData；

机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、同步时间子缓冲区nVClockBuf；

传感数据同步完成位置索引值nG_BufsaveIdx、传感数据写入位置索引值nVBufSaveIdx、待同步缓冲区段长度值nVbuf；

传感数据旧值SensorDataOld，

传感数据获取时间全局值nG_ClockSync_F，

传感数据获取时间本地新值ClockSync_F，

传感数据获取时间本地旧值ClockSync_F_old，

传感数据获取时间本地初值ClockSync_F_ini，

传感数据写入位置偏移值nBias_Vbuf，

传感数据写入位置偏移总量值nBiasN_Vbuf，

工作数据获取时间值ClockSync_W。

如图1(a)和图3所示，机械臂工作数据子缓冲区nG_WorkDataBuf用于存储工作数据WorkData、传感数据子缓冲区nG_SensorDataBuf用于存储传感数据SensorData、同步时间子缓冲区nVClockBuf用于存储工作数据获取时间ClockSync_W，这三组子缓冲区的容量值均为长度值nG_BufLength，数据结构均为数组型，数组元素索引均从0开始。

可读取数据位置索引值BufLoadIdx，用于表示可供读取数据的开始索引位置；传感数据同步完成位置索引值nG_BufsaveIdx，用于标识已完成同步处理的缓冲区段中的最末端索引位置；传感数据写入位置索引值nVBufSaveIdx，用于标识供最新获得的机械臂工作数据WorkData执行写入操作的缓冲区索引位置；待同步缓冲区段长度值nVbuf，用于标识等待进行同步匹配处理的缓冲区段实际长度。

如图4所示，机械臂工作数据快速获取模块嵌入于机械臂的主任务程序的循环体中连续执行，从而高速地获取所述工作数据，以保证具有高速连续特征的机械臂运动任务的准确执行和实时监控；所述主任务程序的循环体紧随轨迹运动指令之后，其循环结束的条件为所述轨迹运动指令对应的实际动作执行完毕；

如图4所示，数据同步匹配模块运行过程为：

S1：数据同步匹配初始化子步骤，此子步骤仅在初始运行阶段执行1次；

待初始化子步骤完成后，生成轨迹运动指令，激活机械臂工作数据快速获取模块并转到步骤S2等；

S2：存储机械臂工作数据子步骤；

S3：处理传感数据子步骤，该子步骤进一步分为S3-1、S3-2、S3-3、S3-4四个细分步骤：

S3-1：判断最新获得的传感数据是否是新数据，若是则继续，若非则返回；

S3-2：依据传感数据获取时间ClockSync_F计算传感数据写入位置偏移总量值nBiasN_Vbuf；

S3-3：依据传感数据写入位置偏移总量值nBiasN_Vbuf，计算和存储传感数据子缓冲区nG_SensorDataBuf中相关区段的数值；

S3-4：数据同步匹配更新操作；

待步骤S3-4结束后，延时等待其他操作结束，并再次循环主任务程序。

数据同步匹配初始化子步骤S1仅在初始运行阶段执行1次；存储机械臂工作数据子步骤S2和处理传感数据子步骤S3组成程序组，嵌入于机械臂的主任务程序的循环体中连续执行，并位于机械臂工作数据快速获取模块之后。

数据同步匹配初始化子步骤S1，其具体方法为：

nVBufSaveIdx＝nG_BufSaveIdx＝0，

nVbuf＝0，

ClockSync_F_old＝ClockSync_F_ini；

存储机械臂工作数据子步骤S2，其具体方法为：

更新传感数据写入位置索引值nVBufSaveIdx：

nVBufSaveIdx＝(nVBufSaveIdx+1)％nG_BufLength；

更新nVbuf：nVbuf＝nVbuf+1；

设置机械臂工作数据子缓冲区nG_WorkDataBuf的值：

nG_WorkDataBuf[nVBufSaveIdx]＝机械臂工作数据WorkData；

设置同步时间子缓冲区nVClockBuf的值：

nVClockBuf[nVBufSaveIdx]＝工作数据获取时间值ClockSync_W。

机械臂工作数据WorkData包含机械臂本体装置的末端姿态，以及与该末端姿态相对应的局部操作姿态。

如图5和图1(a)所示，机械臂工作数据WorkData包含机械臂本体装置的末端姿态，以及与该末端姿态相对应的局部操作姿态，即机械臂工作数据WorkData除了可以用于存储机械臂本体装置的末端姿态以外，还可以用于存储与该末端姿态相对应的电脑显示屏转轴的旋转角度。在该实例中，电脑显示屏转轴的旋转角度范围为0度至150度，因此设定机械臂本体装置的局部操作轨迹，使得机械臂本体装置带动电脑显示屏在该区间内运动；在轨迹的起始位置，电脑显示屏与键盘组件相贴合，即电脑显示屏转轴的旋转角度为0度；在轨迹的终止位置，电脑显示屏转轴的旋转角度到达最大值150度，即图中用实线表示的显示屏位置；若在该轨迹运行过程中的某时刻检测轨迹的执行情况，则该轨迹在该时刻对应的位置即为轨迹的当前位置，即图中用虚线表示的显示屏位置。机械臂本体装置的局部操作轨迹，其信息即对应于机械臂本体装置的一系列的末端姿态；而与末端姿态相对应的电脑显示屏转轴的旋转角度，即被定义为局部操作姿态。在该实例中，局部操作姿态的数值区间即为0度至150度，完整区间数据即为该区间的长度值150。

轨迹运动指令，即用于生成机械臂本体装置的局部操作轨迹的命令。轨迹运动指令对应的轨迹编号由机械臂操作系统管理并负责生成，记录为nMoveID，例如具体值为15，则下一条轨迹运动指令对应的轨迹编号为16。nMoveID起始位置对应的翻转角度为0度，nMoveID对应的完整翻转角度区间为150度。机械臂本体装置沿着该轨迹运动，并带动电脑显示屏沿转轴翻转。在该过程中，通过机械臂操作系统实时获取机械臂本体装置的轨迹执行数据为nMoveID_Now，例如其值为15.6，则得到该运动轨迹的执行比例为nMoveID_Now-nMoveID＝15.3-15＝0.3＝30％，进而计算获得机械臂工作数据即电脑显示屏转轴的当前旋转角度WorkData为WorkData＝0+(15.6-15)*150＝90(度)。

机械臂本体装置是否正在按照该轨迹运行的判断条件为：轨迹编号nMoveID不为空值，且有nMoveID_Now≤nMoveID+1。

如图4所示的处理传感数据子步骤S3，其细分步骤S3-1的具体方法为：

先令ClockSync_F＝nG_ClockSync_F，再判断ClockSync_F与ClockSync_F_old是否相等，若不相等，则传感数据SensorData是新数据，若相等则不是新数据。

如图4所示的处理传感数据子步骤S3，其细分步骤S3-2的具体方法如图6所示：

先令nBiasN_Vbuf＝0，再对如下条件进行判断，若该条件满足，则nBiasN_Vbuf的值自加1，直至该条件不满足：

ClockSync_F≥nVClockBuf[(nG_BufsaveIdx+nBiasN_Vbuf)％nG_BufLength]且nBiasN_Vbuf<nVbuf。

如图6所示，nBiasN_Vbuf由0开始不断增加，直至nVClockBuf[(nG_BufsaveIdx+nBiasN_Vbuf)的值∈[t₁,t₂)，则此时的nBiasN_Vbuf的值即为最终所得。

如图4所示的处理传感数据子步骤S3，其细分步骤S3-3的具体方法为：

若有ClockSync_F_old等于ClockSync_F_ini，则处理如下：若nBiasN_Vbuf等于1，则将传感数据SensorData的值直接存储至nG_SensorDataBuf[nG_BufSaveIdx]，若nBiasN_Vbuf不等于1，则对于nBias_Vbuf∈[0,nBiasN_Vbuf-1]，有nG_SensorDataBuf[nG_BufSaveIdx+nBias_Vbuf]＝SensorData*nBias_Vbuf/(nBiasN_Vbuf-1)。

若ClockSync_F_old不等于ClockSync_F_ini，则对nBias_Vbuf∈[0,nBiasN_Vbuf-1]，有nG_SensorDataBuf[nIdxA]＝SensorDataOld+nCompA*(nVClockBuf[nIdxA]-ClockSync_F_old)，其中，nIdxA＝(nG_BufSaveIdx+nBias_Vbuf)％nG_BufLength，nCompA＝(SensorData-SensorDataOld)/(ClockSync_F-ClockSync_F_old)；

如图4所示的处理传感数据子步骤S3，其细分步骤S3-4的具体方法为：依次令

SensorDataOld＝SensorData，

ClockSync_F_old＝ClockSync_F，

nG_BufSaveIdx＝(nG_BufSaveIdx+nBiasN_Vbuf)％nG_BufLength，

nVbuf＝nVbuf-nBiasN_Vbuf。

传感数据SensorData由外部传感器装置感知获得，在单次采样及进行常规数据处理后，即通过通信总线接口发送至机械臂操作系统中的通信数据缓冲区；若外部传感器装置的具体形式为空间多维力传感器，其测量维数为n，则传感数据SensorData为由n个值组成的向量，例如测量维数为6的空间6维力传感器，其传感数据为[Fx,Fy,Fz,Tx,Ty,Tz]；若通信数据缓冲区发生数据溢出，则优先删除其中最早的数据。

传感数据获取时间全局值nG_ClockSync_F的写入操作仅在传感器数据读取模块中被执行1次，其读取操作仅在数据同步匹配模块中被执行1次，以最大程度地提升互斥保护。

如图7所示，数据存储中心的数据用于在外部监控系统中进行保存、显示与后处理，图7所示即为外部监控系统的软件界面。

机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、传感数据同步完成位置索引值nG_BufsaveIdx、长度值nG_BufLength，用于在外部监控系统中进行信息同步后的显示及后处理。

传感数据SensorData、机械臂工作数据WorkData的值，用于在外部监控系统中进行高速实时地显示，如图7中所示的SensorData和WorkData数据框，即实时地显示最新获得的传感数据SensorData的值，和机械臂工作数据WorkData的值。

外部监控系统与机械臂控制器间通过通信总线接口建立通信连接，以访问相关数据，机械臂端的相关通信任务由机械臂操作系统进行管理，通信数据传输由外部监控系统主动发起。

机械臂工作数据快速获取模块的设计机制，保证了其以最快的速度实时获取机械臂工作数据WorkData，即优先保证对于高速连续运动条件下的相关运动数据的获取。

传感器数据读取模块的设计机制，使得在尽量少地占用机械臂系统的硬件资源和软件资源的前提下，将传感数据SensorData的读取延迟降低到设定的时间范围内。

机械臂工作数据WorkData的计算获取频率，高于传感数据SensorData的采样获取频率，数据同步匹配模块实现了这两种数据间的同步，如图7中左侧显示的曲线，即为同步处理后的SensorData-WorkData关系曲线，另一方面，由于数据同步处理和数据批量传输需要一定时间，该同步数据的显示可能略滞后于右侧的实时显示，例如图中曲线部分的WorkData的最大值42小于右侧WorkData框中的数据值45。如图3所示，外部监控系统可将缓冲区中BufLoadIdx至nG_BufSaveIdx中的数据批量化读出以用于同步曲线的显示，并随后更新BufLoadIdx的值为nG_BufSaveIdx；而外部监控系统中的WorkData数据框则可从缓冲区中nVBufSaveIdx位置读取最新的取值。

外部监控系统的任务，低频率、批量化地获取同步数据缓冲区中的数据，从而进一步降低对于机械臂系统的资源占用。机械臂系统的硬件资源和软件资源占用量的上限，以不影响机械臂系统的正常运行为原则，依据机械臂系统的实际硬件配置进行设定。

机械臂应用程序集和外部监控系统共同实现了机械臂本体装置在高速运动条件下的工作状态获取、外部感知、数据同步匹配、外部监控多种类型任务间的协同。

以上所述，仅是本发明的较佳实施例而已，并非对本发明的结构及控制方式做任何形式上的限制。凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明的技术方案的范围内。

Claims (10)

1.一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述机械臂系统包含机械臂硬件子系统和机械臂软件子系统；

所述机械臂硬件子系统包含机械臂本体装置、机械臂控制器和外部传感器装置；所述机械臂控制器包含通信总线接口；所述外部传感器装置与所述机械臂控制器间通过所述通信总线接口建立通信连接，所述外部传感器装置高速周期化地感知采样并发送传感数据，其感知采样与传感数据发送频率由用户预先设定；所述外部传感器装置的具体形式包括空间多维力传感器，所述空间多维力传感器安装于所述机械臂本体装置的末端；

所述机械臂软件子系统运行于所述机械臂控制器之上；所述机械臂软件子系统包含机械臂操作系统、机械臂应用程序集，和数据存储中心；所述通信总线接口由所述机械臂操作系统负责管理，所述机械臂操作系统包含通信数据缓冲区，该缓冲区用于高速地存储从所述通信总线接口中获得的传感数据SensorData；所述机械臂应用程序集包含传感器数据读取模块、机械臂工作数据快速获取模块、数据同步匹配模块；所述传感器数据读取模块周期化地从所述通信数据缓冲区中读取所述传感数据SensorData，若获取到有效数据则将当前时刻记为传感数据获取时间全局值nG_ClockSync_F，并将其存储至同步数据缓冲区，所述同步数据缓冲区包括机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、同步时间子缓冲区nVClockBuf；所述机械臂工作数据快速获取模块用于快速计算获取与机械臂实际工作任务直接关联的机械臂工作数据WorkData，若获取到有效数据则将当前时刻记为工作数据获取时间值ClockSync_W；所述数据同步匹配模块用于将所述传感数据SensorData和所述机械臂工作数据WorkData进行同步匹配。

2.根据权利要求1所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述机械臂应用程序集中的模块具有不同的实时性需求，随之对应于不同的程序运行方式；

所述机械臂工作数据快速获取模块嵌入于机械臂的主任务程序的循环体中连续执行，从而高速地获取所述机械臂工作数据，使得具有高速连续特征的机械臂运动任务得以执行和实时监控；所述主任务程序的循环体紧随轨迹运动指令之后，其循环结束的条件为所述轨迹运动指令对应的实际动作执行完毕；

将所述传感器数据读取模块每次被调用激活并开始运行时的系统全局时间记为T_{f_global}；所述传感器数据读取模块的单次预计执行时长记为T_{f_predict}，该模块每隔时长T_f被激活执行，按需设定T_f使得T_{f_predict}<T_f；若该模块的单次实际执行时长T_{f_actual}超过了T_f，则当其运行时长等于T_f时，该模块不会被重复激活执行，而是直至本次运行结束，在此情况下，该模块下次被激活执行的时刻为T_{f_global}+(n+1)*T_f，n为整数且有n*T_f<T_{f_actual}<(n+1)*T_f；

所述传感器数据读取模块每隔时长T_f被激活执行的具体方式包括：通过所述机械臂操作系统中包含的时钟周期化激活执行，和通过其它周期化触发的条件激活执行。

3.根据权利要求1所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述数据存储中心，存储的数据包括：

所述传感数据SensorData、机械臂工作数据WorkData；

所述机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、同步时间子缓冲区nVClockBuf，这三组子缓冲区的容量值均为长度值nG_BufLength，数据结构均为数组型，数组元素索引均从0开始；

传感数据同步完成位置索引值nG_BufsaveIdx，用于标识已完成同步处理的缓冲区段中的最末端索引位置；

传感数据写入位置索引值nVBufSaveIdx，用于标识供最新获得的所述机械臂工作数据WorkData执行写入操作的缓冲区索引位置；

待同步缓冲区段长度值nVbuf，用于标识等待进行同步匹配处理的缓冲区段实际长度；

传感数据旧值SensorDataOld，

传感数据获取时间全局值nG_ClockSync_F，

传感数据获取时间本地新值ClockSync_F，

传感数据获取时间本地旧值ClockSync_F_old，

传感数据获取时间本地初值ClockSync_F_ini，

传感数据写入位置偏移值nBias_Vbuf，

传感数据写入位置偏移总量值nBiasN_Vbuf，

工作数据获取时间值ClockSync_W。

4.根据权利要求3所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述数据同步匹配模块，其运行过程包括：

S1：数据同步匹配初始化子步骤；

S2：存储机械臂工作数据子步骤；

S3：处理传感数据子步骤，该子步骤进一步分为S3-1、S3-2、S3-3、S3-4四个细分步骤；

S3-1：判断最新获得的所述传感数据是否是新数据，若是则继续，若非则返回；

S3-2：依据所述传感数据获取时间ClockSync_F计算所述传感数据写入位置偏移总量值nBiasN_Vbuf；

S3-3：依据所述传感数据写入位置偏移总量值nBiasN_Vbuf，计算和存储所述传感数据子缓冲区nG_SensorDataBuf中相关区段的数值；

S3-4：数据同步匹配更新操作；

所述数据同步匹配初始化子步骤S1仅在初始运行阶段执行1次；所述存储机械臂工作数据子步骤S2和所述处理传感数据子步骤S3组成程序组，嵌入于机械臂的主任务程序的循环体中连续执行，并位于所述机械臂工作数据快速获取模块之后。

5.根据权利要求4所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述数据同步匹配初始化子步骤S1，其具体方法为：

将所述传感数据写入位置索引值nVBufSaveIdx、所述传感数据同步完成位置索引值nG_BufSaveIdx、所述待同步缓冲区段长度值nVbuf的值均置为0，

将传感数据获取时间本地初值ClockSync_F_ini的值赋给所述传感数据获取时间本地旧值ClockSync_F_old；

所述存储机械臂工作数据子步骤S2，其具体方法为：

将所述传感数据写入位置索引值nVBufSaveIdx的值自加1后，再对所述nG_BufLength进行取余运算，将得到的结果赋值给nVBufSaveIdx，

将所述待同步缓冲区段长度值nVbuf的值自加1，

将所述机械臂工作数据WorkData存储至所述机械臂工作数据子缓冲区nG_WorkDataBuf数组中的第nVBufSaveIdx项，

将所述工作数据获取时间值ClockSync_W存储至所述同步时间子缓冲区nVClockBuf数组中的第nVBufSaveIdx项。

6.根据权利要求5所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述机械臂工作数据WorkData包含所述机械臂本体装置的末端姿态，以及与该末端姿态相对应的局部操作姿态；

所述机械臂工作数据快速获取模块的工作方式具体为：

每一次执行所述机械臂本体装置的局部操作运动轨迹时，设置与该轨迹运动指令相对应的轨迹编号，记录为nMoveID；所述nMoveID的值随轨迹的数量而同步递增，其编号由所述机械臂操作系统管理并负责生成；在所述机械臂本体装置按照该轨迹运行过程中，通过所述机械臂操作系统实时获取所述机械臂本体装置的轨迹执行数据nMoveID_Now，得到该运动轨迹的执行比例为nMoveID_Now-nMoveID，计算获得所述机械臂工作数据WorkData为：

WorkData＝nMoveID起始位置对应的工作数据+(nMoveID_Now-nMoveID)*nMoveID对应的完整区间数据；

所述机械臂本体装置是否正在按照该轨迹运行的判断条件为：

所述轨迹编号nMoveID不为空值，且有nMoveID_Now≤nMoveID+1；

所述nMoveID对应的完整区间数据，为与该轨迹所对应的所述局部操作姿态的数值区间长度。

7.根据权利要求4所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述处理传感数据子步骤S3，其细分步骤S3-1的具体方法为：

先令ClockSync_F＝nG_ClockSync_F，再判断ClockSync_F与ClockSync_F_old是否相等，若不相等，则所述传感数据SensorData是新数据，若相等则不是新数据；

所述处理传感数据子步骤S3，其细分步骤S3-2的具体方法为：

先令nBiasN_Vbuf＝0，再对如下条件进行判断，若该条件满足，则nBiasN_Vbuf的值自加1，直至该条件不满足：

ClockSync_F≥nVClockBuf[(nG_BufsaveIdx+nBiasN_Vbuf)％nG_BufLength]且nBiasN_Vbuf<nVbuf；

所述处理传感数据子步骤S3，其细分步骤S3-3的具体方法为：

若有ClockSync_F_old等于ClockSync_F_ini，则处理如下：若nBiasN_Vbuf等于1，则将所述传感数据SensorData的值直接存储至nG_SensorDataBuf[nG_BufSaveIdx]，若nBiasN_Vbuf不等于1，则对于nBias_Vbuf∈[0,nBiasN_Vbuf-1]，按如下插值方式计算获得nG_SensorDataBuf数组中索引位置为nIdxA的值：

nG_SensorDataBuf[nIdxA]＝SensorData*nBias_Vbuf/(nBiasN_Vbuf-1)，其中，nIdxA的值为(nG_BufSaveIdx+nBias_Vbuf)％nG_BufLength；

若ClockSync_F_old不等于ClockSync_F_ini，则对nBias_Vbuf∈[0,nBiasN_Vbuf-1]，按如下插值方式计算获得nG_SensorDataBuf数组中索引位置为nIdxA的值：

nG_SensorDataBuf[nIdxA]＝SensorDataOld+nCompA*(nVClockBuf[nIdxA]-ClockSync_F_old)，其中，nIdxA的值为(nG_BufSaveIdx+nBias_Vbuf)％nG_BufLength，nCompA＝(SensorData-SensorDataOld)/(ClockSync_F-ClockSync_F_old)；

所述处理传感数据子步骤S3，其细分步骤S3-4的具体方法为：

将所述传感数据SensorData的值赋给所述传感数据旧值SensorDataOld，

将所述传感数据获取时间本地新值ClockSync_F的值赋给所述传感数据获取时间本地旧值，

将所述传感数据同步完成位置索引值nG_BufSaveIdx与所述传感数据写入位置偏移总量值nBiasN_Vbuf进行求和，再对所述长度值nG_BufLength进行取余计算，将得到的结果赋值给所述待同步缓冲区段长度值nVBuf。

8.根据权利要求1所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述传感数据SensorData由所述外部传感器装置感知获得，在单次采样及进行常规数据处理后，即通过所述通信总线接口发送至所述机械臂操作系统中的所述通信数据缓冲区；若所述外部传感器装置的具体形式为空间多维力传感器，其测量维数为n，则传感数据SensorData为由n个值组成的向量；若所述通信数据缓冲区发生数据溢出，则优先删除其中最早的数据；

所述传感数据获取时间全局值nG_ClockSync_F的写入操作仅在所述传感器数据读取模块中被执行1次，其读取操作仅在所述数据同步匹配模块中被执行1次，以提升互斥保护。

9.根据权利要求3所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述数据存储中心的数据用于在外部监控系统中进行保存、显示与后处理；

所述机械臂工作数据子缓冲区nG_WorkDataBuf、传感数据子缓冲区nG_SensorDataBuf、传感数据同步完成位置索引值nG_BufsaveIdx、长度值nG_BufLength，用于在所述外部监控系统中进行信息同步后的显示及后处理；

所述传感数据SensorData、机械臂工作数据WorkData的值，用于在所述外部监控系统中进行高速实时地显示；

所述外部监控系统与所述机械臂控制器间通过所述通信总线接口建立通信连接，以访问相关数据，机械臂端的相关通信任务由所述机械臂操作系统进行管理，通信数据传输由所述外部监控系统主动发起。

10.根据权利要求9所述的一种高速连续运动中的机械臂系统多任务协同与信息同步方法，其特征在于：

所述机械臂工作数据快速获取模块的设计机制，使得其快速实时获取所述机械臂工作数据WorkData，即优先对高速连续运动条件下的相关运动数据进行获取；

所述传感器数据读取模块的设计机制，使得在小于设定所述机械臂系统的硬件资源和软件资源占用量的前提下，将所述传感数据SensorData的读取延迟降低到设定的时间范围内；

所述机械臂工作数据WorkData的计算获取频率，高于所述传感数据SensorData的采样获取频率，所述数据同步匹配模块用于实现这两种数据间的同步；

所述外部监控系统的任务，低频率、批量化地获取所述同步数据缓冲区中的数据，从而使得对于所述机械臂系统的资源占用量小于设定值；

所述机械臂应用程序集和所述外部监控系统共同用于实现所述机械臂本体装置在高速运动条件下的工作状态获取、外部感知、数据同步匹配、外部监控多种类型任务间的协同。

Images