CN111026164A - 一种机器人目标跟踪轨迹规划方法 - Google Patents

Abstract

本发明公开了一种机器人目标跟踪轨迹规划方法，对视觉设备和机器人建立跟随用户坐标系与机器人基坐标系之间的描述关系，根据传送带实时反馈位置和反馈速度以及视觉设备拍摄的目标物体位置，通过坐标系转换关系，得到目标物体在机器人基坐标系中的位置，再建立机器人末端TCP点及目标物体之间的几何位置关系，对机器人相对于传送带上目标物体行进路线的平行方向及垂直方向的位移进行解耦，根据规划位移进行预测调整，同时进行实时速度的规划补偿，最终合成机器人的同步跟踪轨迹。本发明方法可以有效削弱机器人在传送带运行垂直方向上力的冲击，降低机器人在同步跟踪到位时的抖动，保证同步跟踪的到位精度，满足机器人对运动平滑性及到位准确度的要求。

Description

一种机器人目标跟踪轨迹规划方法

技术领域

本发明涉及一种机器人轨迹规划方法，具体说是一种机器人目标跟踪轨迹规划方法。

背景技术

随着工业自动化的不断发展，工业机器人的应用日趋广泛，机器人的作业变得愈加有效，工业中越来越多的作业更适合于应用机器人自动化，这就要求机器人的控制更加智能和有效，能够模拟人类进行更多的作业工序。在自动分拣和物料搬运等应用领域，需要机器人对目标进行动态跟踪并保持同步运动，进而同步作业，这就要求机器人能够对移动的目标进行实时跟踪，保证同步精度并且机器人运动平滑顺畅。

在这类应用中，一般包含传送带、机器人及视觉设备，传送带以预设的速度持续运动。使用时首先进行标定，建立视觉设备与机器人基坐标系之间的用户坐标系描述关系，目标物体在移动的传送带上，经过视觉设备的拍摄范围时，视觉设备启动拍摄，经过图像处理，将拍到的目标物体的位置发送给机器人，机器人根据当前传送带反馈位置和反馈速度以及跟随用户坐标系的描述，实时计算出目标物体在机器人跟随用户坐标系中的位置，再通过坐标系转换关系，得到目标物体在机器人基坐标系下的位置，然后进行目标检测及跟踪。

机器人进行目标跟踪时，通常需要与其它工序配合，当机器人完成其它工序后，首先到达示教好的等待点，再开始目标跟踪检测。机器人根据视觉设备拍摄到的目标物体位置以及传送带实际反馈位置和反馈速度，通过坐标系转换，得到目标物体在机器人基坐标系下的位置，当检测到目标物体进入机器人工作空间范围内时，机器人启动目标跟踪流程，实时跟踪目标物体，直到机器人末端TCP点到达目标物体位置上方，并且机器人末端TCP点的运行速度与传送带的运行速度大小相等且方向相同。此时，机器人与目标物体保持同步运动，即：相对于跟随用户坐标系，机器人与目标物体之间是静止的。在此状态下，机器人可完成同步动作，例如：同步分拣作业。完成同步动作后，机器人释放同步状态，进入减速抬升状态，到达跟随结束点。其后，机器人切换至控制器中的其它指令，进行后续工序作业。

当机器人启动目标跟踪流程后，在到达跟随同步状态前的过程中，机器人在跟随用户坐标系下的运动可以分解为相对于传送带运行的垂直方向和平行方向的运动，机器人在传送带运行方向的运动为平行方向运动，机器人在Y、Z方向及姿态旋转的运动，统称为垂直方向的运动。由于传送带来料时目标物体的位置不固定，使得机器人在垂直方向上存在位移分量。因此，机器人需要在垂直方向上从等待点运动到传送带上目标物体的行进路线上，并且机器人运动到行进路线上后要保证垂直方向的速度分量为零；与此同时，机器人需要在平行方向上从等待点运动到目标物体在行进路线上的实时位置，并且与目标物体的速度相同，这样才能与目标物体保持同步。

根据动量公式P＝mV，冲量公式I＝Ft，以及动量与冲量的关系I＝mdV，由于速度矢量的存在，机器人在运动方向上会产生力的冲击。其中：P为动量，m为质量，V为速度，I为冲量，F为受到的力，t为时间，dV为速度的变化量。在跟随用户坐标系下，机器人到达跟随同步状态前，由于在垂直方向及平行方向上均有位移，为了保证同步，机器人在这两个方向上都会产生速度矢量。特别是机器人在垂直方向上的速度矢量，由于和目标物体实际运动方向有夹角，存在垂直方向的作用力，导致运动时产生冲击。

中国发明专利《一种使用视觉系统对直线传送带上工件定位的装置和方法》(专利申请号201410336533.7)公开了一种使用视觉系统对直线传送带上工件定位的装置和方法，将工件放置于拍摄区域中并静止不动，视觉系统拍摄并记录位置后，传送带传输工件并于一段距离后停止，机器人经计算后对工件实施动作。该专利中视觉系统拍摄及机器人抓取时，传送带上的工件均处于静止状态，机器人虽然实现对工件的抓取，但由于传送带的启停导致加工节拍慢、运行效率低。

中国发明专利《一种基于视觉引导的工业机器人工件定位抓取方法及系统》(专利申请号201410073766.2)公开了一种基于视觉引导的工业机器人工件定位抓取方法；中国发明专利《基于视觉的机器人动态跟踪抓取方法及系统》(专利申请号201610587245.8)公开了一种基于视觉的机器人动态跟踪抓取方法及系统，都实现了机器人对工件的动态跟踪，但都是对跟踪轨迹的整体规划，未考虑机器人相对于目标物体行进路线的各个运动分量的影响。

中国发明专利《一种高效率锯条自动化抓取系统》(专利申请号201611117482.4)公开了一种高效率锯条自动化抓取系统。该专利在坐标系标定时分析了传送带与机器人在X、Y方向的相对位置关系，但跟踪时对X、Y同步处理，也未对Y方向进行额外调整。

因此，现有的机器人同步控制方法，通常仅考虑了从跟随开始阶段到同步阶段满足位移同步和速度同步的条件，这类方法对跟踪路径一般都是采用整体规划，在机器人接近同步前的时刻，在垂直方向上仍有速度、加速度及加加速度，产生运动冲击，引起机器人本体的抖动，影响机器人的使用寿命，同时造成精度丢失，对机器人跟踪的同步效果及到位精度产生直接影响，无法满足机器人高精度同步跟踪的应用需求。

发明内容

本发明所要解决的技术问题，在于克服现有技术存在的缺陷，提出一种机器人目标跟踪轨迹规划方法。该方法可以有效削弱机器人在传送带运行垂直方向上力的冲击，降低机器人在同步跟踪到位时的抖动，保证同步跟踪的到位精度，满足机器人同步跟踪应用中对运动平滑性及到位准确度的要求。

本发明的方法，对视觉设备和机器人建立跟随用户坐标系与机器人基坐标系之间的描述关系，根据传送带实时反馈位置和反馈速度以及视觉设备拍摄的目标物体位置，通过坐标系转换关系，得到目标物体在机器人基坐标系中的位置，再建立机器人末端TCP点及目标物体之间的几何位置关系，对机器人相对于传送带上目标物体行进路线的平行方向及垂直方向的位移进行解耦，根据规划位移进行预测调整，同时进行实时速度的规划补偿，最终合成机器人的同步跟踪轨迹。本发明方法预测调整的设计核心在于，调整后机器人在垂直方向的运行时间要与平行方向的运行时间相近，并且垂直方向要先与平行方向到达，保证机器人同步到位时在垂直方向的速度、加速度、加加速度均已提前到达0。

本发明的一种机器人目标跟踪轨迹规划方法，具体实现如下：

步骤1、机器人控制器与传送带建立连接，机器人控制器实时接收传送带当前的反馈位置和反馈速度。

步骤2、视觉系统与机器人控制器之间建立通讯连接，使得机器人控制器可以接收视觉设备拍摄的目标物体的位置。

步骤3、建立跟踪用户坐标系，使得机器人能够正确识别视觉系统拍摄的目标物体位置。采用机器人控制器中的用户坐标系标定功能，建立跟踪用户坐标系，跟踪用户坐标系的X方向为传送带的行进方向，竖直向上为Z方向，通过右手定则即可确定Y方向。

步骤4、设置传送带的运行速度Vconv；设置机器人运动参数，包括：最大运行速度、加速度、加加速度等运动参数；设置跟随参数，包括：跟随的左边界Xmin、跟随的右边界Xmax以及跟踪同步精度e。

步骤5、视觉系统拍摄目标物体，将其在跟踪用户坐标系下的位置，发给机器人控制器；机器人控制器同时记录拍照时传送带的初始位置。

步骤6、机器人控制器实时监测目标物体的实际位置，并进行动态跟踪：

步骤6.1、机器人控制器根据拍照后传送带的位移增量，计算目标物体在t时刻的实际位置Pu(t)。传送带运行方向即为目标物体行进方向，目标物体在X方向上的位移变化量即为传送带的位移增量，其它方向坐标值与拍照时记录的初始值相同。

步骤6.2、实时检测目标物体当前实际位置与跟随边界值Xmin、Xmax的相对关系，当目标物体运动到跟随边界范围内时，启动跟随流程。

步骤6.3、根据坐标系转换关系，计算出目标物体在机器人基坐标系下的实际位置Pw(t)，当该位置进入机器人工作空间可达范围内时，机器人从等待点开始运动，进行动态跟踪。

步骤6.4、根据机器人与目标物体之间的位置关系，对机器人的运动进行预测。

建立机器人末端TCP点和目标物体之间的几何关系，对机器人相对于目标物体行进路线的平行方向及垂直方向的位移进行解耦，分别得到平行方向及垂直方向的位移分量Dx、Dy。根据Dx、Dy以及传送带的运行速度，判断机器人TCP与目标物体的位置关系。

当Dy<＝e时，机器人TCP已经在目标物体的行进路线上，仅在X方向进行同步跟踪；反之，机器人TCP与目标物体行进路线之间存在较大位移差，机器人必须从等待点运动到目标物体行进路线上才可与之同步。因此，当Dy>e时，根据预设的最大速度、加速度、加加速度等运动参数，对位移进行速度预规划。这里的速度预规划可以采用多种方式，本发明仅以S型曲线为例说明。

根据垂直方向的位移Dy以及设定的运动参数，在垂直方向上建立速度规划函数，y(t)＝F(Dy,t)，计算出机器人在垂直方向的最大运行时间Ty，规划时垂直方向的起始速度为0，终止速度为0。由于机器人必须运动到目标物体的行进路线上，因此，机器人至少需要Ty时间才能与目标物体保持同步状态。

再对机器人在平行方向的运动进行规划，机器人在平行方向的运行时间Tx＝Ty+Tx^。其中：Tx^为Ty时刻后机器人平行方向与目标物体位置同步且速度达到Vconv所需的时间。

根据平行方向的起始速度、终止速度以及设定的运动参数，在X方向上建立速度规划函数，x(t)＝G(Vx(Ty),Vconv,t)，计算机器人在X方向的运行时间Tx，其中：Vx(Ty)为Ty时刻机器人在平行方向的速度。当Vx(Ty)<Vconv时，机器人在X方向仍需加速跟踪，故Tx^>0。

步骤6.5、根据以上速度预规划计算出的时间，对机器人在平行方向或垂直方向的运动进行调整。

当Ty<Tx时，说明当机器人在垂直方向上运动到达目标物体行进路线上时，在X方向仍未运动完成，此时仍处于对目标物体的追踪状态，此时在垂直方向上的速度矢量已经变为0，不会产生冲击。当Ty>Tx时，说明机器人在X方向已经和目标物体保持同步状态，但在垂直方向还未到达目标物体行进路线上，此时在垂直方向仍有较大的速度矢量存在。

根据以上预测，当Ty>Tx-C时，对Tx建立调整曲线，Kx(t)＝t*(Ty+C)/Tx，(其中：C为常数且C至少为一个插补周期，C值根据现场工况设定，使得Ty与Tx不能相差太多)，则x(t)＝G(Vx(Ty),Vconv,Kx(t))，对X方向运动时间进行缩放，使得X方向时间到达后，垂直方向上速度矢量也运动完成，避免垂直方向上因速度矢量存在而导致力的冲击。当Ty<＝Tx-C时，对Ty建立调整曲线，Ky(t)＝t*(Tx-C)/Ty，则y(t)＝F(Dy,Ky(t))，对垂直方向运动时间进行缩放，使得垂直方向能够以接近X运动完成的时间到达，既保证了位移到位，又避免了调整前垂直方向比X方向过快到达而导致的由路径加减速不协调引起的抖动。

步骤6.6、增加实时速度规划项。

虽然传送带以预设的速度运行，但实际中一般存在速度波动，为了保证机器人TCP对目标物体的实时跟踪性，对X方向的运动规划还包括实时速度的跟踪项，具体如下：

在传送带运行的任意时刻，以机器人在X方向的当前速度Vx为开始速度、以当前目标物体实时速度Vc为终止速度，进行X方向的实时运动的速度规划xs(t)＝H(Vx,Vc,t)。

步骤6.7、机器人动态规划，对目标物体进行实时跟踪。

根据机器人在X方向和垂直方向的预测调整项以及在X方向的实时速度规划项，对机器人的运动进行合成。其中：机器人在X方向的运动由预测调整项x(t)和实时速度规划项xs(t)组成，即：x^(t)＝a*x(t)+b*xs(t)，其中：a、b是非零常数且满足关系a+b＝1。最终，机器人按此合成作用进行插补，得到离散的插补点，这些插补点形成机器人TCP的跟踪轨迹。

综上所述，当机器人按照该方法运动到达Tx后，在垂直方向上速度为零且与目标物体位移为零；在X方向上与目标物体保持同速且位移为零。即：机器人TCP点到达传送带上目标物体的行进路线上，此时仅可能有X方向的轨迹跟踪。当传送带速度有波动时，仅需在X方向以机器人当前速度作为起始速度、以目标物体运行速度作为终止速度进行规划。当机器人运行速度与目标物体运行速度相同，且位置误差在同步精度e范围内时，机器人与目标物体达到同步跟踪状态。

达到同步跟踪状态后，机器人与目标物体保持相对静止，此时可根据应用需求进行同步抓取或其它操作，操作完成后，机器人进入同步释放阶段并减速至0。此后，机器人可切换至控制器中的其它指令，完成后续加工作业。机器人也可以运动回到等待点，进入下一次目标跟踪流程。

本发明提供的机器人目标跟踪轨迹规划方法，在建立视觉系统与机器人基坐标系之间的描述关系后，根据机器人TCP与目标物体实际位置的相对关系，对机器人在跟踪用户坐标系中相对于传送带运行的平行方向和垂直方向的运动进行解耦，从而考虑了其在各自方向上的受力，通过对平行方向及垂直方向运动时间的预测及调整，既避免了垂直方向运动时间短而在平行方向仍有运动，两者运动时间不协调带来的抖动；也避免了垂直方向运动时间长而平行方向到位时仍有垂直方向速度矢量，从而产生动量带来力的冲击。同时，本发明方法也考虑了传送带速度波动带来的影响并对目标物体进行了实时规划补偿。本发明方法有效避免了机器人同步跟踪到位时因速度矢量带来的抖动，提高了机器人同步跟踪时的到位精度及稳定性，有效适用于机器人传送带动态跟踪的应用场合。

附图说明

图1为机器人目标跟踪的传送带应用示意图。

图2为本发明方法实施例中机器人预测的位移关系图。

图3为本发明方法实施例中预测调整前的时间关系图。

图4为本发明方法实施例中预测调整后的时间关系图。

具体实施方式

下面结合附图和具体实施例，对本发明方法作进一步详细说明。

以SCARA机器人的一次传送带目标跟踪过程为例，说明本发明方法的具体实现方法。

S1、根据现场情况安装传送带、机器人及视觉系统。将机器人控制器与传送带建立连接，机器人控制器实时接收传送带当前的反馈位置和反馈速度。

S2、将视觉系统与机器人控制器之间建立通讯连接，使得机器人控制器可以接收视觉设备拍摄的目标物体的位置。

S3、采用机器人控制器中的用户坐标系标定功能，建立跟踪用户坐标系，使得机器人能够正确识别视觉系统拍摄的目标物体位置。该用户坐标系的X方向为传送带的行进方向，竖直向上为Z方向，通过右手定则确定Y方向。

S4、在机器人控制器中设置传送带的运行速度Vconv为200mm/s；设置机器人运动的必要参数：最大运行速度设为1000mm/s、加速度设为10000mm/s²、加加速度设为100000mm/s³；设置跟随的必要参数：根据标定出的跟随用户坐标系的原点位置以及机器人的现场工作空间，设置跟随的左边界Xmin＝100mm、跟随的右边界Xmax＝800mm以及跟踪同步精度e＝1mm。

S5、视觉系统拍摄目标物体，将其在跟随用户坐标系下的位置Pu0(200,10,0,0)，发给机器人控制器；机器人控制器同时记录拍照时传送带的初始位置。

S6、机器人控制器实时监测目标物体的实际位置，并进行动态跟踪。

S6.1、机器人控制器根据拍照后传送带的位移增量，计算目标物体在t时刻的实际位置Pu(t)。传送带运行方向即为目标物体行进方向，目标物体在X方向上的位移变化量即为传送带的位移增量，其它方向坐标值与拍照时记录的初始值相同。

S6.2、实时检测目标物体当前实际位置与跟随边界值Xmin、Xmax的相对关系，当目标物体运动到跟随边界范围内时，启动跟随流程。

S6.3、根据坐标系转换关系，计算出目标物体在机器人基坐标系下的实际位置Pw(t)，当该位置进入机器人工作空间可达范围内时，机器人从等待点Pstandby开始运动，进行动态跟踪。

S6.4、根据机器人与目标物体之间的位置关系，对机器人的运动进行预测。

建立机器人末端TCP点和目标物体之间的几何关系，对机器人相对于目标物体行进路线的平行方向及垂直方向的位移进行解耦，分别得到平行方向及垂直方向的位移分量，本实施例中假设机器人与目标物体姿态相同、Z方向在同一平面内，仅在X、Y方向有位移，分别为Dx＝10mm、Dy＝50mm。根据Dx、Dy以及传送带的运行速度，判断机器人TCP与目标物体的位置关系。

由于Dy>e，故机器人TCP与目标物体行进路线之间存在较大位移差，机器人必须从等待点运动到目标物体行进路线上才可与之同步。根据预设的最大速度、加速度、加加速度等运动参数，对位移进行速度预规划。这里的速度预规划以S型曲线为例说明。

根据垂直方向的位移Dy以及设定的运动参数，在Y方向上建立速度规划函数，y(t)＝F(50,t)，计算出机器人在Y方向的最大运行时间Ty，规划时Y方向起始速度为0，终止速度为0。由于机器人必须运动到目标物体的行进路线上，因此，机器人至少需要Ty时间才能与目标物体保持同步状态。

再对机器人在平行方向的运动进行规划，机器人在平行方向的运行时间Tx＝Ty+Tx^。其中：Tx^为Ty时刻后机器人平行方向与目标物体位置同步且速度达到Vconv所需的时间。

根据平行方向的起始速度、终止速度以及设定的运动参数，在X方向上建立速度规划函数，x(t)＝G(Vx(Ty),Vconv,t)，计算机器人在X方向的运行时间Tx，其中：Vx(Ty)为Ty时刻机器人在平行方向的速度。当Vx(Ty)<Vconv时，机器人在X方向仍需加速跟踪，故Tx^>0。

S6.5、根据以上速度预规划计算出的时间，对机器人在平行方向或垂直方向的运动进行调整。

在本实施例中，Dy>Dx，Ty>Tx，说明机器人在X方向已经和目标物体保持同步状态，但在Y方向还未到达目标物体行进路线上，此时在Y方向仍有较大的速度矢量存在。

根据以上预测，由于Ty>Tx-C，故对Tx建立调整曲线，Kx(t)＝t*(Ty+C)/Tx，(根据工况取C为2个插补周期)，则x(t)＝G(Vx(Ty),Vconv,Kx(t))，对X方向运动时间进行缩放，使得X方向时间到达后，Y方向也运动完成，避免Y方向上因速度矢量存在而导致力的冲击。

S6.6、增加实时速度规划项。

虽然传送带以预设的速度Vconv＝200mm/s运行，但实际中一般存在速度波动，实际速度可能在200mm/s附近波动，为了保证机器人TCP对目标物体的实时跟踪性，对X方向的运动规划还包括实时速度的跟踪项，具体如下：

在传送带运行的任意时刻，以机器人在X方向的当前速度Vx为开始速度、以当前目标物体实时速度Vc为终止速度，进行X方向的实时运动的速度规划xs(t)＝H(Vx,Vc,t)。

S6.7、机器人动态规划，对目标物体进行实时跟踪。

根据机器人在X方向和Y方向的预测调整项以及在X方向的实时速度规划项，对机器人的运动进行合成。其中：机器人在X方向的运动由预测调整项x(t)和实时速度规划项xs(t)组成，即：x^(t)＝a*x(t)+b*xs(t)，本实施例中：取a＝0.5、b＝1-a。最终，机器人按此合成作用进行插补，得到离散的插补点，这些插补点形成机器人TCP的跟踪轨迹。

综上所述，当机器人按照该方法运动到达Tx后，在Y方向上速度为零且与目标物体位移为零；在X方向上与目标物体保持同速且位移为零。即：机器人TCP点到达传送带上目标物体的行进路线上，此时仅可能有X方向的轨迹跟踪。当传送带速度有波动时，仅需在X方向以机器人当前速度作为起始速度、以目标物体运行速度作为终止速度进行规划。当机器人运行速度与目标物体运行速度相同，且位置误差在同步精度e范围内时，机器人与目标物体达到同步跟踪状态。

达到同步跟踪状态后，机器人与目标物体保持相对静止，此时可根据应用需求进行同步抓取或其它操作，操作完成后，机器人进入同步释放阶段并减速至0。此后，机器人可切换至控制器中的其它指令，完成后续加工作业。在本实施例中：机器人运动回到等待点，进入下一次目标跟踪流程，机器人控制器等待视觉系统再次拍摄传送带上的物体，并将目标物体位置发送给控制器，继续进行下一次的目标跟踪。如此往复，机器人对传送带上各个来料进行同步跟踪、抓取及释放，完成整个作业流程。

本发明方法提供的机器人目标跟踪轨迹规划方法，通过对机器人相对于目标物体的位移进行预测及调整，并叠加实时跟踪项的补偿，使得机器人能够对传送带上的物体进行同步跟踪，并且机器人在同步跟踪过程中的运动平滑，特别是当机器人与目标物体同步时，不会因传送带运行方向以外的运动矢量带来抖动，保证了同步到位时机器人的跟踪精度及稳定性，适用于高精度的机器人应用需求。

以上所述仅为本发明的优选实施方式，并不构成对本发明保护范围的限定。任何在本发明的精神和原则之内所做的修改、等同替换和方案改进等，均应包含在本发明的权利要求保护范围之内。

Claims (1)

1.一种机器人目标跟踪轨迹规划方法，其步骤如下：

步骤1、机器人控制器与传送带建立连接，机器人控制器实时接收传送带当前的反馈位置和反馈速度；

步骤2、视觉系统与机器人控制器之间建立通讯连接，用于机器人控制器接收视觉设备拍摄的目标物体的位置；

步骤3、建立跟踪用户坐标系

采用机器人控制器中的用户坐标系标定功能，建立跟踪用户坐标系，跟踪用户坐标系的X方向为传送带的行进方向，竖直向上为Z方向，通过右手定则即可确定Y方向；

步骤4、设置传送带的运行速度Vconv；设置机器人运动参数：最大运行速度、加速度、加加速度；设置跟随参数：跟随的左边界Xmin、跟随的右边界Xmax以及跟踪同步精度e；

步骤5、视觉系统拍摄目标物体，将其在跟踪用户坐标系下的位置，发给机器人控制器；机器人控制器同时记录拍照时传送带的初始位置；

步骤6、机器人控制器实时监测目标物体的实际位置，并进行动态跟踪：

步骤6.1、机器人控制器根据拍照后传送带的位移增量，计算目标物体在t时刻的实际位置Pu(t)；传送带运行方向即为目标物体行进方向，目标物体在X方向上的位移变化量即为传送带的位移增量，其它方向坐标值与拍照时记录的初始值相同；

步骤6.2、实时检测目标物体当前实际位置与跟随边界值Xmin、Xmax的相对关系，当目标物体运动到跟随边界范围内时，启动跟随流程；

步骤6.3、根据坐标系转换关系，计算出目标物体在机器人基坐标系下的实际位置Pw(t)，当该位置进入机器人工作空间可达范围内时，机器人从等待点开始运动，进行动态跟踪；

步骤6.4、根据机器人与目标物体之间的位置关系，对机器人的运动进行预测(预规划)

建立机器人末端TCP点和目标物体之间的几何关系，对机器人相对于目标物体行进路线的平行方向及垂直方向的位移进行解耦，分别得到平行方向及垂直方向的位移分量Dx、Dy；根据Dx、Dy以及传送带的运行速度，判断机器人TCP与目标物体的位置关系：

当Dy<＝e时，机器人TCP已经在目标物体的行进路线上，仅在X方向进行同步跟踪；反之，机器人TCP与目标物体行进路线之间存在较大位移差，机器人必须从等待点运动到目标物体行进路线上才可与之同步；

当Dy>e时，根据预设的最大速度、加速度、加加速度运动参数，对位移进行速度预规划：

根据垂直方向的位移Dy以及设定的运动参数，在垂直方向上建立速度规划函数，y(t)＝F(Dy,t)，计算出机器人在垂直方向的最大运行时间Ty，规划时垂直方向的起始速度为0，终止速度为0；

再对机器人在平行方向的运动进行规划，机器人在平行方向的运行时间Tx＝Ty+Tx^；其中：Tx^为Ty时刻后机器人平行方向与目标物体位置同步且速度达到Vconv所需的时间；

根据平行方向的起始速度、终止速度以及设定的运动参数，在X方向上建立速度规划函数，x(t)＝G(Vx(Ty),Vconv,t)，计算机器人在X方向的运行时间Tx，其中：Vx(Ty)为Ty时刻机器人在平行方向的速度；当Vx(Ty)<Vconv时，机器人在X方向仍需加速跟踪，故Tx^>0；

步骤6.5、根据步骤6.4速度预规划计算出的时间，对机器人在平行方向或垂直方向的运动进行调整：

当Ty<Tx时，说明当机器人在垂直方向上运动到达目标物体行进路线上时，在X方向仍未运动完成，此时仍处于对目标物体的追踪状态，此时在垂直方向上的速度矢量已经变为0，不会产生冲击；

当Ty>Tx时，说明机器人在X方向已经和目标物体保持同步状态，但在垂直方向还未到达目标物体行进路线上，此时在垂直方向仍有较大的速度矢量存在；

根据以上预测，当Ty>Tx-C时，对Tx建立调整曲线，Kx(t)＝t*(Ty+C)/Tx，其中：C为常数且C至少为一个插补周期，C值根据现场工况设定，使得Ty与Tx不能相差太多，则x(t)＝G(Vx(Ty),Vconv,Kx(t))，对X方向运动时间进行缩放，使得X方向时间到达后，垂直方向上速度矢量也运动完成，避免垂直方向上因速度矢量存在而导致力的冲击；

当Ty<＝Tx-C时，对Ty建立调整曲线，Ky(t)＝t*(Tx-C)/Ty，则y(t)＝F(Dy,Ky(t))，对垂直方向运动时间进行缩放，使得垂直方向能够以接近X运动完成的时间到达；

步骤6.6、增加实时速度规划项

为了保证机器人TCP对目标物体的实时跟踪性，对X方向的运动规划还包括实时速度的跟踪项，具体如下：

在传送带运行的任意时刻，以机器人在X方向的当前速度Vx为开始速度、以当前目标物体实时速度Vc为终止速度，进行X方向的实时运动的速度规划xs(t)＝H(Vx,Vc,t)；

步骤6.7、机器人动态规划，对目标物体进行实时跟踪。

根据机器人在X方向和垂直方向的预测调整项以及在X方向的实时速度规划项，对机器人的运动进行合成；其中：机器人在X方向的运动由预测调整项x(t)和实时速度规划项xs(t)组成，即：x^(t)＝a*x(t)+b*xs(t)，其中：a、b是非零常数且满足关系a+b＝1；

最终，机器人按此合成作用进行插补，得到离散的插补点，这些插补点形成机器人TCP的跟踪轨迹。

Images