基于S曲线加减速的运动目标动态跟踪方法、系统及装置

摘要

本发明涉及一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，该方法包括：获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。本发明充分利用机器人的工作空间，基于时间最短的原则动态跟踪传送带上的工件，具有良好的动态性能和抓取效率。

说明书

技术领域

本发明属于计算机数控系统运动控制的技术领域，尤其是涉及一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置。

背景技术

机器人的一个常见应用领域是运动目标的跟踪，实现如抓取、包装、分拣等应用。例如，在机器人分拣中，将DELTA并联机械手应用于视觉分拣系统，能够显著提高生产效率，增强机器人的环境适应能力。但是，一个高效、合理的抓取模式是视觉分拣系统正常工作的前提。

典型的视觉分拣系统包含的装置主要有：机器人、直线传送带、视觉系统和工件。传送带以恒定速度从上游方向往下游方向直线传输，视觉系统位于传送带上方，使用视觉系统对直线传送带上的工件定位。传送带运行过程中，视觉系统检测处于拍摄区域中的工件，视觉系统将拍摄区域中的工件的位置信息拍摄并记录，将位置信息传递给控制器，控制器将数据传输给机器人，机器人对工件位置数据进行分析并预测抓取位置，实现准确定位，机器人随即规划运动轨迹，完成相应的如动态抓取等动作。传统采用定点等待抓取的方式，不能充分利用机器人的工作空间，难以提高分拣效率。

现有的方法中，中国专利文件(申请号201010184309.2)提出了根据工件在传送带上的分布密度来调整传送带速度的控制思想，以保证机器人总是处在最快抓取速度状态，并进行了简单的理论分析，但该方法实现较为困难，并且不符合对生产节拍有要求的生产过程。

中国专利文件(申请号201610222488.1)提出使用修正梯形算法作为机器人加减速算法，虽然该算法计算简单，但由于修正梯形算法自身时间分配的不合理性，导致它在短时间内的运动具有跳跃性，长距离运动表现疲软的特点，动态特性不足，难以保证高效率的抓取。

综上所述，针对现有技术如何进一步有效动态跟踪运动目标以及提高跟踪效率的问题，尚缺乏有效的解决方案。

发明内容

针对现有技术中存在的不足，解决现有技术中如何有效动态跟踪运动目标以及提高跟踪效率的问题，本发明提供了一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，有效实现运动目标的动态跟随，充分利用跟踪装置的工作空间，基于时间最短的原则动态跟踪运动目标，具有良好的动态性能和跟踪效率。

本发明的第一目的是提供一种基于S曲线加减速的运动目标动态跟踪方法。

为了实现上述目的，本发明采用如下一种技术方案：

一种基于S曲线加减速的运动目标动态跟踪方法，该方法包括：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

在本发明中，充分利用跟踪装置的工作空间，根据S曲线加减速算法划分出跟踪装置不同的工作区域，根据S型速度曲线体现的跟踪装置不同的运动情况逐个分析跟踪装置的工作区域，由于每次跟踪前跟踪装置的初始位置可能不同，在动态跟踪前重新计算确定工作区域参数，保证动态跟踪的准确性；目标物体运行轨迹与跟踪装置位置关系不同，经过的跟踪装置工作区域的不同，判断目标物体运行轨迹与跟踪装置位置关系，确定目标物体运行轨迹经过的至少一个的跟踪装置工作区域，确定所求变量的取值范围和构建跟踪装置不同工作区域对应的动态跟踪方程，分别求解动态跟踪方程实现动态跟随，提高动态性能和抓取效率。

本发明充分利用跟踪装置的工作空间，基于整体时间最短的原则动态跟踪传送带上的工件，具有良好的动态性能和抓取效率。

作为进一步的优选方案，所述跟踪装置跟踪至目标物体相遇点的跟踪过程运行轨迹为门字形轨迹，其中，跟踪过程中的水平运动阶段采用S曲线加减速算法，并将跟踪过程中的水平运动阶段划分跟踪装置的工作区域。

作为进一步的优选方案，假设S曲线的初始速度大于末端速度，所述跟踪装置的工作区域根据S曲线加减速算法中S型速度曲线是否存在匀加速段、匀减速段和/或匀速段在水平面将跟踪过程进行划分。

作为进一步的优选方案，所述跟踪装置的工作区域包括跟踪装置初始位置由内向外辐射的第一工作区域、第二工作区域、第三工作区域和第四工作区域；

所述第一工作区域为跟踪装置的S型速度曲线不存在匀加速段、匀减速段和匀速段的工作区域；

所述第二工作区域为跟踪装置的S型速度曲线存在匀减速段、不存在匀加速段和匀速段的工作区域；

所述第三工作区域为跟踪装置的S型速度曲线存在匀加速段和匀减速段、不存在匀速段的工作区域；

所述第四工作区域为跟踪装置的S型速度曲线存在匀速段和/或匀加速段、匀减速段的工作区域。

在本发明中，根据S曲线加减速算法中S型速度曲线是否存在匀加速段、匀减速段和/或匀速段在水平面将跟踪过程进行划分，将跟踪装置的工作空间进行详尽的划分，充分利用跟踪装置的工作空间，从而提高跟踪效率。

作为进一步的优选方案，所述工作区域初始化参数包括工作区域半径；

所述工作区域半径为跟踪装置由初始位置移动到该工作区域外围边界时的位移，包括第一工作区域半径、第二工作区域半径、第三工作区域半径和第四工作区域半径。

作为进一步的优选方案，所述判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域的具体步骤为：

若目标物体运行轨迹与跟踪装置初始位置距离小于第一工作区域半径，则目标物体运行轨迹经过的跟踪装置工作区域依次为第四工作区域、第三工作区域、第二工作区域、第一工作区域、第二工作区域、第三工作区域和第四工作区域；

若目标物体运行轨迹与跟踪装置初始位置距离大于第一工作区域半径小于第二工作区域，则目标物体运行轨迹经过的跟踪装置工作区域依次为第四工作区域、第三工作区域、第二工作区域、第三工作区域和第四工作区域；

若目标物体运行轨迹与跟踪装置初始位置距离大于第二工作区域半径小于第三工作区域半径，则目标物体运行轨迹经过的跟踪装置工作区域依次为第四工作区域、第三工作区域和第四工作区域；

若目标物体运行轨迹与跟踪装置初始位置距离大于第三工作区域半径小于第四工作区域半径，则目标物体运行轨迹经过的跟踪装置工作区域为第四工作区域。

作为进一步的优选方案，所述基于整体时间最短原则求解动态跟踪方程的具体步骤为：

依次求解目标物体运行轨迹经过的跟踪装置工作区域的动态跟踪方程，

若经过的跟踪装置工作区域的动态跟踪方程有解，则为跟踪装置与目标物体的相遇点，进行动态跟踪；

否则，继续求解下一个经过的跟踪装置工作区域的动态跟踪方程；

若所有目标物体运行轨迹经过的跟踪装置工作区域的动态跟踪方程均无解，结束跟踪。

作为进一步的优选方案，跟踪装置工作区域的动态跟踪方程可采用数值分析或物理模型进行求解；

所述物理模型的求解方法具体步骤为：

建立跟踪装置和目标物体的物理模型，所述物理模型中根据跟踪装置至目标物体运行轨迹垂足将目标物体运行轨迹分为上半部分和下半部分；

分别对上半部分和下半部分根据跟踪装置和目标物体的运行物理意义建立位移时间图像求解动态跟踪方程。

在本发明中，单纯采用数值分析的数学求解法求解动态跟踪方程，分析区间的单调性以及解的分布计算量大，且较难分析；为了分析其区间内解的唯一性，采用基于实际工况简化的物理模型与数学位移时间图像相结合的方式，快速、直观的分析动态跟踪方程的有解性，进而求解动态跟踪方程。

本发明的第二目的是提供一种计算机可读存储介质。

为了实现上述目的，本发明采用如下一种技术方案：

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行以下处理：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

本发明的第三目的是提供一种基于S曲线加减速的运动目标动态跟踪装置。

为了实现上述目的，本发明采用如下一种技术方案：

一种基于S曲线加减速的运动目标动态跟踪装置，采用计算终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行以下处理：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

本发明的第四目的是提供一种基于S曲线加减速的运动目标动态跟踪系统。

为了实现上述目的，本发明采用如下一种技术方案：

一种基于S曲线加减速的运动目标动态跟踪系统，包括：

目标物体及目标物体参数采集装置，所述目标物体参数采集装置用于采集目标物体的参数，包括目标物体的位置和速度，并将目标物体的参数传输至基于S曲线加减速的运动目标动态跟踪装置；

跟踪装置及跟踪装置参数采集装置，所述跟踪装置用于动态跟踪目标物体，所述跟踪装置参数采集装置用于采集跟踪装置的参数，包括跟踪装置的位置和速度，并将跟踪装置的参数传输至基于S曲线加减速的运动目标动态跟踪装置；

基于S曲线加减速的运动目标动态跟踪装置，用于：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

本发明的有益效果：

1、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，充分利用跟踪装置的工作空间，根据S曲线加减速算法划分出跟踪装置不同的工作区域，根据S型速度曲线体现的跟踪装置不同的运动情况逐个分析跟踪装置的工作区域，由于每次跟踪前跟踪装置的初始位置可能不同，在动态跟踪前重新计算确定工作区域参数，保证动态跟踪的准确性；且采用S曲线加减速算法作为跟踪装置的加减速算法保证了跟踪装置运动的连续性，适合较长距离运动；

2、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，根据目标物体运行轨迹与跟踪装置位置关系不同，经过的跟踪装置工作区域的不同，判断目标物体运行轨迹与跟踪装置位置关系，确定目标物体运行轨迹经过的至少一个的跟踪装置工作区域，确定所求变量的取值范围和构建跟踪装置不同工作区域对应的动态跟踪方程，分别求解动态跟踪方程实现动态跟随，提高动态性能和抓取效率；

3、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，根据S曲线加减速算法中S型速度曲线是否存在匀加速段、匀减速段和/或匀速段在水平面将跟踪过程进行划分，将跟踪装置的工作空间进行详尽的划分，充分利用跟踪装置的工作空间，从而提高跟踪效率；

4、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，为了分析其区间内解的唯一性，采用基于实际工况简化的物理模型与数学位移时间图像相结合的方式，快速、直观的分析动态跟踪方程的有解性，进而求解动态跟踪方程。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为本发明中的方法流程图；

图2为本发明中的几何模型示意图；

图3为本发明中的七段的S形加减速曲线示意图；

图4为本发明中的工作空间分区示意图；

图5为本发明中实施例2的方程解物理分析模型示意图；

图6为本发明中实施例2的上半部分t—S曲线示意图；

图7为本发明中实施例2的下半部分t—S曲线示意图；

图8为本发明中的算法具体执行过程示意图。

具体实施方式：

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本实施例使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

针对现有技术中存在的不足，解决现有技术中如何有效动态跟踪运动目标以及提高跟踪效率的问题，本发明提供了一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，有效实现运动目标的动态跟随，充分利用跟踪装置的工作空间，基于时间最短的原则动态跟踪运动目标，具有良好的动态性能和跟踪效率。

在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面结合附图与实施例对本发明作进一步说明。

实施例1：

本实施例1的目的是提供一种基于S曲线加减速的运动目标动态跟踪方法。

为了实现上述目的，本发明采用如下一种技术方案：

如图1所示，

一种基于S曲线加减速的运动目标动态跟踪方法，该方法包括：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

在本发明中，充分利用跟踪装置的工作空间，根据S曲线加减速算法划分出跟踪装置不同的工作区域，根据S型速度曲线体现的跟踪装置不同的运动情况逐个分析跟踪装置的工作区域，由于每次跟踪前跟踪装置的初始位置可能不同，在动态跟踪前重新计算确定工作区域参数，保证动态跟踪的准确性；目标物体运行轨迹与跟踪装置位置关系不同，经过的跟踪装置工作区域的不同，判断目标物体运行轨迹与跟踪装置位置关系，确定目标物体运行轨迹经过的至少一个的跟踪装置工作区域，确定所求变量的取值范围和构建跟踪装置不同工作区域对应的动态跟踪方程，分别求解动态跟踪方程实现动态跟随，提高动态性能和抓取效率。

本发明充分利用跟踪装置的工作空间，基于整体时间最短的原则动态跟踪传送带上的工件，具有良好的动态性能和抓取效率。

在本实施例中以DELTA并联机械手视觉分拣系统为例对基于S曲线加减速的运动目标动态跟踪方法进行详细说明，在DELTA并联机械手视觉分拣系统中，目标物体即工件放置于匀速直线运动的传送带上，传送带上方设置第一位置采集装置，本实施例中采用工业相机，也可采用本领域技术人员公知的其他位置采集装置采集传送带上工件的位置信息，DELTA并联机械手设置于传送带一侧，以DELTA并联机械手末端位置作为跟踪装置位置信息，使跟踪准确。使用第二位置采集装置或DELTA并联机械手中自带的定位装置采集DELTA并联机械手末端位置。

在本实施例中，DELTA并联机械手末端点由放置点运行至抓取工件的抓取点的路径为门字形轨迹，其中水平运动阶段采用S曲线加减速算法，定义：S型速度曲线的起点速度v_s，终点速度v_e，指令速度v_com，加速度限制a_lim，加加速度限制j_lim，S型速度曲线七段运行时间分别为t₁,t₂,t₃,t₄,t₅,t₆,t₇，门字形轨迹两侧竖直运动总时间t'，门字形轨迹运动总时间t，传送带速度v_传，其中门字形轨迹运动总时间t：

t＝t₁+t₂+t₃+t₄+t₅+t₆+t₇+t'(1)

基于S曲线加减速的运动目标动态跟踪方法的具体步骤为：

步骤(1)：获取同一时刻的工件初始位置和DELTA并联机械手末端初始位置，其中工件初始位置为D(x_d,y_d)，DELTA并联机械手末端初始位置为E(x_e,y_e)；通过几何关系构建模型，DELTA并联机械手末端点跟踪至工件的相遇点位置F(x_f,y_f)，构建几何模型，如图2所示。

根据几何模型的几何关系得，

其中，x_e为DELTA并联机械手末端初始位置横坐标，y_d为工件初始位置横坐标，L_DE为DELTA并联机械手末端初始位置距工件初始位置的水平距离，L_DF为工件初始位置至工件预测被抓取位置的位移，L_EF为DELTA并联机械手末端初始位置至工件预测被抓取位置的水平位移。

步骤(2)：根据S曲线加减速算法中S型速度曲线的运动模式将跟踪过程中的水平运动阶段划分跟踪装置的工作区域。

如图3所示为七段的S型位移曲线、速度曲线、加速度曲线和加加速度曲线，传统S型曲线包括7段，加加速段，匀加速段，减加速段，匀速段，加减速段，匀减速段，减减速度段，但是其中匀加速段，匀速段和匀减速段是不一定都存在的，需要根据加速段实际最大速度v_max2'(加速度最大时对应的速度)与指令速度v_com的关系来判断是否存在匀加速段，根据减速段的最大速度v_max1'与指令速度v_com的大小关系来判断是否存在匀减速段。根据加速段与减速段位移的和与总位移的大小关系判断是否存在匀速段。

在本实施例中，所述跟踪装置的工作区域根据S曲线加减速算法中S型速度曲线是否存在匀加速段、匀减速段和/或匀速段在水平面将跟踪过程进行划分。根据S曲线规划算法，按照速度大或者小的一侧做完整的三角形加减速，两侧加速到最大速度v_max，并比较与指令速度v_com的关系的思路来划分工作区域。具体步骤包括：

步骤(2-1)：取v_s，v_e中较小的一方作完整三角形加减速，求出能达到的最大速度v_max1'，若v_max1'＜v_com，则此时的位移范围半径为R₁；若v_max1'＞v_com，则以v_com为最大速度，计算R₁；

步骤(2-2)：若v_max1'＜v_com，取v_s，v_e中较大的一方作完整的三角形加速，求出v_max2'，若v_max2'＜v_com，则此时的位移范围半径为R₂；若v_max2'＞v_com，则以v_com为最大速度，计算R₂；

步骤(2-3)：若v_max2'＜v_com，以v_com计算工作区域，其半径为R₃。

步骤(2-4)：机器人的最大工作空间其半径为R₄。

在本实施例中，假设v_s＞v_e，基于步骤(2-1)-步骤(2-3)，将DELTA并联机械手末端点的工作空间分为四部分，并计算相应的所述工作区域初始化参数：工作区域半径；所述工作区域半径为跟踪装置由初始位置移动到该工作区域外围边界时的位移，包括第一工作区域半径R₁、第二工作区域半径R₂、第三工作区域半径R₃和第四工作区域半径R₄。如图4所示，其中：

所述跟踪装置的工作区域包括跟踪装置初始位置由内向外辐射的第一工作区域0—R1、第二工作区域R1—R2、第三工作区域R2—R3和第四工作区域R3—R4；

第一工作区域0—R1：v_e作完整的三角形加减速；所述第一工作区域为跟踪装置的S型速度曲线不存在匀加速段、匀减速段和匀速段的工作区域；

第二工作区域R1—R2：v_s作完整三角形加减速；所述第二工作区域为跟踪装置的S型速度曲线存在匀减速段、不存在匀加速段和匀速段的工作区域；

第三工作区域R2—R3：没有匀速段，最大速度为v_max；所述第三工作区域为跟踪装置的S型速度曲线存在匀加速段和匀减速段、不存在匀速段的工作区域；

第四工作区域R3—R4：v_com为最大速度，有匀速段。所述第四工作区域为跟踪装置的S型速度曲线存在匀速段和/或匀加速段、匀减速段的工作区域。

在本发明中，根据S曲线加减速算法中S型速度曲线是否存在匀加速段、匀减速段和/或匀速段在水平面将跟踪过程进行划分，将跟踪装置的工作空间进行详尽的划分，充分利用跟踪装置的工作空间，从而提高跟踪效率。

每个工作区域构建动态跟踪方程的方法分别为：

①第一工作区域0—R1，不存在匀加速段、匀减速段和匀速段的工作区域；

即v_e作完整的三角形加速，最大速度满足v_max∈[0,v_max1']；

此时，t₂＝t₄＝t₆＝0(3)

时间计算如下，

故

根据S曲线位移计算方法，得：

加速段位移

减速段位移

L_EF＝L₁+L₂(9)

根据几何关系转化可得方程，

L_DE²+v_传²t²-(L₁+L₂)²-2(x_e-x_d)v_传t＝0(10)

将式(6)-式(9)代入式(10)，得

在区间v_max∈[0,v_max1']内使用数值方法求解方程(11)。

②第二工作区域R1—R2，跟踪装置的S型速度曲线存在匀减速段、不存在匀加速段和匀速段的工作区域，即v_s作完整三角形加速，最大速度满足v_max∈[v_max1',v_max2']；

在v_e一侧会出现时间不定的匀减速段，具体时间计算如下：

则

由位移关系，得加速段位移

减速段位移

L_EF＝L₁+L₂(18)

根据几何关系转化可得方程，

L_DE²+v_传²t²-(L₁+L₂)²-2(x_e-x_d)v_传t＝0(19)

将式(15)-式(18)代入式(19)，得，

在区间v_max∈[v_max1',v_max2']内使用数值方法求解方程(20)。

③第三工作区域R2—R3，跟踪装置的S型速度曲线存在匀加速段和匀减速段、不存在匀速段的工作区域；最大速度满足v_max∈[v_max2',v_com]；

此时，

t₄＝0(22)

则

根据S曲线位移的关系，

根据图2几何关系得，L_EF＝L₁+L₂(28)

根据几何关系转化可得方程，

L_DE²+v_传²t²-(L₁+L₂)²-2(x_e-x_d)v_传t＝0(29)

化简得，A₃v_max⁴+B₃v_max³+C₃v_max²+D₃v_max+E₃＝0(30)

其中，

在区间v_max∈[v_max2',v_com]内使用数值方法求解方程(29)。

④第四工作区域R3—R4：跟踪装置的S型速度曲线存在匀速段和/或匀加速段、匀减速段的工作区域；即v_com为最大速度，有匀速段。

对于S曲线各段时间，先假设能完成三角形加速，则

若t₂＞0，则t₂存在；

若t₂＜＝0，则重新计算t₁和t₃，且t₂＝0，此时，

同理，若t₆＞0，则t₆存在；

若t₆＜＝0，则t₆＝0，重新计算t₅和t₇，此时，

设加速段距离为L₁，减速段距离为L₂，加速段和减速段距离总和L”，匀速段位移为L₃，加速和减速的总时间为t”，则

L”＝L₁+L₂(43)

t”＝t₁+t₂+t₃+t₅+t₆+t₇(44)

设t”'＝t'+t”，则

L₃＝v_com(t-t'-t”)＝v_com(t-t”')(45)

根据图2几何关系得，L_EF＝L”+L₃(46)

根据几何关系转化可得方程，

(L₃+L”)²+2(x_e-x_d)v_传t-v_传²t²-L_DE²＝0(47)

化简得，A₄t²+B₄+C₄＝0(48)

其中A₄＝v_com²-v_传²(49)

B₄＝2(v_comL”-v_com²t”'+(x_e-x_d)v_传)(50)

C₄＝v_com²(t”')²-2v_comL”t”'+(L”)²-L_DE²(51)

使用数值方法求解方程(47)。

步骤(3)：判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域，其具体步骤为：

步骤(3-1)：判断目标物体运行轨迹与跟踪装置初始位置距离是否大于第三工作区域半径小于第四工作区域半径，若目标物体运行轨迹与跟踪装置初始位置距离大于第三工作区域半径小于第四工作区域半径，则目标物体运行轨迹经过的跟踪装置工作区域为第四工作区域；否则进入下一步；

步骤(3-2)：判断目标物体运行轨迹与跟踪装置初始位置距离是否大于第二工作区域半径小于第三工作区域半径，若目标物体运行轨迹与跟踪装置初始位置距离大于第二工作区域半径小于第三工作区域半径，则目标物体运行轨迹经过的跟踪装置工作区域依次为第四工作区域、第三工作区域和第四工作区域；否则进入下一步；

步骤(3-3)：判断目标物体运行轨迹与跟踪装置初始位置距离是否大于第一工作区域半径小于第二工作区域，若目标物体运行轨迹与跟踪装置初始位置距离大于第一工作区域半径小于第二工作区域，则目标物体运行轨迹经过的跟踪装置工作区域依次为第四工作区域、第三工作区域、第二工作区域、第三工作区域和第四工作区域；否则进入下一步；

步骤(3-4)：判断目标物体运行轨迹与跟踪装置初始位置距离是否小于第一工作区域半径，若目标物体运行轨迹与跟踪装置初始位置距离小于第一工作区域半径，则目标物体运行轨迹经过的跟踪装置工作区域依次为第四工作区域、第三工作区域、第二工作区域、第一工作区域、第二工作区域、第三工作区域和第四工作区域。

步骤(4)：分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

基于整体时间最短原则求解动态跟踪方程的具体步骤为：

步骤(4-1)：依次求解目标物体运行轨迹经过的跟踪装置工作区域的动态跟踪方程，

步骤(4-2)：判断经过的跟踪装置工作区域的动态跟踪方程是否有解；

步骤(4-3)：若经过的跟踪装置工作区域的动态跟踪方程有解，则为跟踪装置与目标物体的相遇点，进行动态跟踪；

否则，继续求解下一个经过的跟踪装置工作区域的动态跟踪方程；

步骤(4-4)：若所有目标物体运行轨迹经过的跟踪装置工作区域的动态跟踪方程均无解，结束跟踪。

在本实施例中，对步骤(3)-步骤(4)结合具体情况进行更详细的说明，如图4所示，根据图4中工件位置与S曲线工作范围的关系，设置α、β、γ、δ四种特殊位置，α情况为目标物体运行轨迹与跟踪装置初始位置距离小于第一工作区域半径，β情况为目标物体运行轨迹与跟踪装置初始位置距离大于第一工作区域半径小于第二工作区域半径，γ情况为目标物体运行轨迹与跟踪装置初始位置距离大于第二工作区域半径小于第三工作区域半径，δ情况为目标物体运行轨迹与跟踪装置初始位置距离大于第三工作区域半径小于第四工作区域半径。

步骤(3)-步骤(4)根据这四种位置分别分析DELTA并联机械手末端点的抓取运动方程，即其动态跟踪方程。抓取过程中，应追求总体时间最短的目标，以实现更多工件的抓取，而不仅仅是DELTA并联机械手末端点的运动时间最短，基于该原则，应当使工件的抓取位置尽量的靠上。从工件匀速运动的角度来分析，这样能减少单个工件的平均抓取时间。

本实施例中假设α、β、γ、δ四种情况都存在，即在最复杂的情况下讨论运动方程的解。

Case1：工件位置处于δ情况

此时，工件在运行过程中一直处于情况④，即第四工作区域R3—R4，跳转到④情况进行计算。

若方程有解，且解在工作范围内，则执行抓取，若无解或者方程解不在工作范围内，则放弃抓取。

Case2：工件位置处于γ情况

γ情况跨越了两种类型的S曲线工作区域，依次求解工件运行轨迹经过的跟踪装置工作区域的动态跟踪方程，具体步骤为：

首先应判断当前的工件位置，然后分不同的情况讨论抓取时间，可根据图4分为以下情况。

1)工件位于γ₁-A₃或F₃-γ₂之间，即第四工作区域R3—R4；

此时应首先考虑在第四工作区域[R₃,R₄]范围内进行抓取，计算方法用情况④，如果无法完成抓取，则进入步骤2)；

2)工件位于A₃-F₃，即第三工作区域，或者步骤1)无法完成抓取

该情况下，DELTA并联机械手末端点运行过程中无匀速段，考虑在第三工作区域[R₂,R₃]范围内进行抓取，计算方法采用情况③；

Case3：工件位置处于β情况

β类型跨越了3个不同的工作区域，因此最多需要讨论3种不同的情况，具体分析如下：

1)工件位于β₁-A₂或F₂-β₂之间，即第四工作区域R3—R4；

此时应首先考虑在第四工作区域[R₃,R₄]范围内进行抓取，计算方法用情况④，如果无法完成抓取，则进入步骤2)；

2)工件位于A₂-B₂之间，即第三工作区域，或位于β₁-A₂但无解的情况

该情况下，DELTA并联机械手末端点运行过程中无匀速段，考虑在[R₂,R₃]范围内进行抓取，计算方法采用情况③，如果无解则进入步骤3)；

3)工件位于B₂-E₂之间，即第二工作区域，或者步骤1)和步骤2)均无解；

该情况下，考虑在第二工作区域[R₁,R₂]范围内进行抓取，计算方法采用情况②；

Case4：工件位置处于α情况

α情况跨越了四个不同的工作范围，因此最多需要讨论4种不同的情况，具体分析如下：

1)工件位于α₁-A₁或F₁-α₂之间，即第四工作区域R3—R4；

此时考虑在第四工作区域[R₃,R₄]范围内进行抓取，计算方法用情况④，如果无法完成抓取，则进入步骤2)；

2)工件位于A₁-B₁或E₁-F₁，，即第三工作区域，或工件位于α₁-A₁但该区域内无解

该情况下，DELTA并联机械手末端点运行过程中无匀速段，考虑在第三工作区域[R₂,R₃]范围内进行抓取，计算方法采用情况③，如果无解则进入步骤3)；

3)工件位于B₁-C₁或D₁-E₁，即第二工作区域，或工件位于α₁-B₁但该区域内无解

该情况下，考虑在第二工作区域[R₁,R₂]范围内进行抓取，计算方法采用情况②，如果无解则进入步骤4)；

4)工件位于C₁-D₁，即第一工作区域，或工件位于α₁-C₁但该区域内无解

该情况下，考虑在第一工作区域[0,R₁]范围内进行抓取，计算方法采用情况①。

在本实施例中，还列举一种具体的抓取情况对整个基于S曲线加减速的运动目标动态跟踪方法进行说明，如图8所示，DELTA并联机械手末端点停留在E点，下一个待抓取的工件在D点。

A)进入任务，

B)进行初始化，判断当前运动参数是否改变，若改变，以E点为圆心，重新计算计算S曲线不同工作区域的半径大小R1-R4，确定情况①②③④。

由于每次的放置点E点都有可能不同，因此每次都要重新计算。

C)根据DELTA并联机械手末端点坐标和工件位置计算G点坐标；

D)根据当前工件的位置和①②③④四种情况的半径判断该工件位置属于α、β、γ、δ哪种情况；

E)同时根据工作范围的圆方程与α、β、γ、δ求出A-F等各处的关键交点坐标，这些交点就是各种子情况的取值范围端点；

针对α、β、γ、δ四种case整理成4个模块，根据Step2中的判断调用其中的模块进行计算；

α、β、γ、δ中每个情况又包含多种区域，对应着Step1求出的4种情况，将①②③④整理成四个子模块，α、β、γ、δ中包含适当的子模块；

F)判断工件当前位置属于①②③④四种情况中的哪种；

G)确定当前工件所处的位置的上下限；

H)调用时间计算函数获取时间，确定该工作区域内能否完成抓取，若能够完成抓取，转入步骤(J)，否则，转入步骤(I)；

I)判断下方是否还有可抓取区域，若有，确定范围上下限，返回步骤(H)重新计算，否则，转入步骤(K)；

J)输出抓取时间，输出能抓取标志位，转入步骤(L)；

K)放弃该工件输出未能抓取标志位，转入步骤(L)；

L)任务完成。

实施例2：

本实施例2的目的是提供一种基于S曲线加减速的运动目标动态跟踪方法。本实施例基于实施例1的基础，增加了求解动态跟踪方程的方法。

跟踪装置工作区域的动态跟踪方程可采用物理模型进行分析，通过数值分析方法进行求解；动态跟踪方程的有解性分析是一个物理问题，且因为式(11)、(20)、(29)、(51)形势复杂，仅从数学的角度来分析区间的单调性以及解的分布计算量大，且较难分析，为了分析其区间内解的唯一性，本实施例中提出一种基于实际工况简化的物理模型与S曲线位移时间图形的方法来分析各方程在取值范围内的单调性。

所述物理模型的求解方法具体步骤为：

步骤(1)：建立跟踪装置和目标物体的物理模型，所述物理模型中根据跟踪装置至目标物体运行轨迹垂足将目标物体运行轨迹分为上半部分和下半部分；

针对实施例1中的①、②、③、④中任一种情况，本实施例2建立如图5所示的物理模型，图5中，竖直线段部分为该位型下工件的沿着传送带运动范围[A,B]，以中间垂足G作为分割点将工作区域分为两部分：上半部分和下半部分。

步骤(2)：分别对上半部分和下半部分根据跟踪装置和目标物体的运行物理意义建立位移时间图像求解动态跟踪方程。

下面分情况讨论解的情况。

1)对于上半部分，即AG部分，DELTA并联机械手末端点末端由E点运动到AG的时间随着工件位置点的下移而减小；而AG段上工件运动中越靠近G点，所需的时间越长。

设DELTA并联机械手末端点开始运动时工件位置为D₁点，工件运行到G点的时刻为t₁，绘制t-S图像如图6所示，其中，粗线为工件运动曲线，细线为DELTA并联机械手末端点运动曲线。

由图6可得，两种曲线都是单调的，可能存在两种情况。

情况1：若机器人运动到G点所需要的时间小于工件运动至G点的时间t₁，则在[D1,G]之间有解，且解唯一，下半部分不用考虑。

情况2：若情况1不成立，则[D1,G]之间无解，需要考虑下半部分求解。

2)对于下半部分，即[G,B]部分，假设当前工件位于D3位置，根据图4建立的模型可观察出，在区间[D3,B]之内，随着工件逐渐向B点靠近，即工件的位移单调增大时，机器人运动的位移单调增大，同时，机器人运动的时间t也在单调增大。

仿照上半区的分析，此处仍可借助t—S曲线来进行分析，S为位置，因为此处S与t的单调性相同，可将横坐标S看作是机器人运动的位移，两者在区间[D3,B]内是等价的。故根据S曲线加减速算法中位移与时间的关系曲线，可以得下半部分的t—S曲线如图7所示。

由图7可得，可能存在两种情况：

情况1：若机器人运动到B所需要的时间小于工件运动至B的时间t₂，则在[D3,B]之间有解，且解唯一。

情况2：若情况1不成立，则[D3,B]之间无解。

综上所述，针对上述①、②、③、④中任一种情况，在对应的区间内，相遇问题若有解则仅存在唯一解，若无解则进行下一个次计算。

在本发明中，单纯采用数值分析的数学求解法求解动态跟踪方程，分析区间的单调性以及解的分布计算量大，且较难分析；为了分析其区间内解的唯一性，采用基于实际工况简化的物理模型与数学位移时间图像相结合的方式，快速、直观的分析动态跟踪方程的有解性，进而求解动态跟踪方程。

本发明不仅可以使用实施例1中的数值分析及实施例2中的物理模型求解动态跟踪方程，还可使用其他本领域技术人员公知的求解方程的方法，例如，二分法、牛顿迭代法等。

实施例3：

本实施例3的目的是提供一种计算机可读存储介质。

为了实现上述目的，本发明采用如下一种技术方案：

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行以下处理：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

在本实施例中，计算机可读记录介质的例子包括磁存储介质(例如，ROM，RAM，USB，软盘，硬盘等)、光学记录介质(例如，CD-ROM或DVD)、PC接口(例如，PCI、PCI-Expres、WiFi等)等。然而，本公开的各个方面不限于此。

实施例4：

本实施例4的目的是提供一种基于S曲线加减速的运动目标动态跟踪装置。

为了实现上述目的，本发明采用如下一种技术方案：

一种基于S曲线加减速的运动目标动态跟踪装置，采用一种计算终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行以下处理：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

本领域技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。

实施例5：

本实施例5的目的是提供一种基于S曲线加减速的运动目标动态跟踪系统。

为了实现上述目的，本发明采用如下一种技术方案：

一种基于S曲线加减速的运动目标动态跟踪系统，包括：

目标物体及目标物体参数采集装置，所述目标物体参数采集装置用于采集目标物体的参数，包括目标物体的位置和速度，并将目标物体的参数传输至基于S曲线加减速的运动目标动态跟踪装置；

跟踪装置及跟踪装置参数采集装置，所述跟踪装置用于动态跟踪目标物体，所述跟踪装置参数采集装置用于采集跟踪装置的参数，包括跟踪装置的位置和速度，并将跟踪装置的参数传输至基于S曲线加减速的运动目标动态跟踪装置；

基于S曲线加减速的运动目标动态跟踪装置，用于：

获取同一时刻的目标物体初始位置和跟踪装置初始位置，预测跟踪装置跟踪至目标物体的相遇点位置，构建几何模型；根据S曲线加减速算法划分跟踪装置的工作区域，计算工作区域初始化参数；

判断目标物体运行轨迹与跟踪装置初始位置关系，确定目标物体运行轨迹经过的跟踪装置工作区域；分别根据几何模型构建跟踪装置不同工作区域的动态跟踪方程，并基于整体时间最短原则求解动态跟踪方程，进行动态跟踪。

本发明的有益效果：

1、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，充分利用跟踪装置的工作空间，根据S曲线加减速算法划分出跟踪装置不同的工作区域，根据S型速度曲线体现的跟踪装置不同的运动情况逐个分析跟踪装置的工作区域，由于每次跟踪前跟踪装置的初始位置可能不同，在动态跟踪前重新计算确定工作区域参数，保证动态跟踪的准确性；且采用S曲线加减速算法作为跟踪装置的加减速算法保证了跟踪装置运动的连续性，适合较长距离运动；

2、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，根据目标物体运行轨迹与跟踪装置位置关系不同，经过的跟踪装置工作区域的不同，判断目标物体运行轨迹与跟踪装置位置关系，确定目标物体运行轨迹经过的至少一个的跟踪装置工作区域，确定所求变量的取值范围和构建跟踪装置不同工作区域对应的动态跟踪方程，分别求解动态跟踪方程实现动态跟随，提高动态性能和抓取效率；

3、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，根据S曲线加减速算法中S型速度曲线是否存在匀加速段、匀减速段和/或匀速段在水平面将跟踪过程进行划分，将跟踪装置的工作空间进行详尽的划分，充分利用跟踪装置的工作空间，从而提高跟踪效率；

4、本发明所述的一种基于S曲线加减速的运动目标动态跟踪方法、系统及装置，为了分析其区间内解的唯一性，采用基于实际工况简化的物理模型与数学位移时间图像相结合的方式，快速、直观的分析动态跟踪方程的有解性，进而求解动态跟踪方程。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。