一种轨迹动态跟踪实现方法与流程

本发明涉及机器人技术领域，尤其涉及一种轨迹动态跟踪实现方法。

背景技术：

在机器人动态作业中，难点在于对运动目标的跟踪。一部分跟踪算法是基于目标的特征检测，建立特征模型，计算复杂且耗时，不适合工业机器人实时、快速抓取的要求。一部分是基于滤波理论的跟踪算法，例如卡尔曼滤波算法，利用线性系统状态方程，根据系统输入对系统状态进行最优估计，更新系统估计状态并不断迭代，它可以较为准确的预测目标的运动状态从而实现跟踪；后续研究者针对非线性系统提出了扩展卡尔曼滤波算法，但其涉及矩阵求导，对处理器而言运算复杂。邹腾跃等提出了基于背景建模的目标检测和基于局部稀疏表达的粒子滤波跟踪方法，并采用反馈机制实现了变焦跟踪，其算法具有较高的准确性和鲁棒性，但牺牲了一定的计算时间。邓明星等提出了改进pid算法跟踪传送带上运动的物体，通过视觉定位和传送带编码器的位置反馈信息，可计算物体的实时位姿，并通过实时调整机器人末端速度和位姿来跟踪传送带上的物体。王峥等提出了基于位置预测的传送带上物体拦截式抓取方法，并分析对比出pid跟踪抓取适用于传送带速度慢的场合，拦截式跟踪抓取适用于传送带速度较快的场合，，pid跟踪在慢速下轨迹曲率较小，效率较高，而快速时轨迹曲率增大，效率较低。

技术实现要素：

本发明的目的就在于为了解决上述问题而提供一种轨迹动态跟踪实现方法。

本发明通过以下技术方案来实现上述目的：

本发明包括工件建模及标定和机器人跟踪过程坐标计算，所述工件建模及标定包括工件基准坐标系建立及标定、工艺点示教，所述工件基准坐标系建立及标定在标定时，先将工件放在相机视野内，拍照并根据转换计算得工件的起始位姿p0即传送带坐标系下，记录此时的编码器读数e1，然后开启传送带使工件运动到机器人工作范围内，记录此时的编码器读数e2，将工件此时的位姿转换到机器人基坐标系下，即为工件基准坐标系base的欧拉角表示，计算如公式(1)所示：

为了后述方便进行坐标变换的表达，将工件基准坐标系到机器人基坐标系的转换关系记为标定的工件基准坐标系相对于机器人基坐标系静止，但是实时的工件局部坐标系track是动态的，由视觉检出和传送带实时位移速度可计算得到；

所述工艺点示教：通过示教工艺点和工件基准坐标系的关系，可以在跟踪时计算运动工件上不同工艺点的实时坐标；标定完基准坐标系后，移动机器人使tcp对准工件轮廓点，分别记录p1p2p3p4在机器人基坐标系下的坐标，完成工艺点的示教；在跟踪该工艺点时，将该坐标映射到运动工件的实时局部坐标系上，机器人即可处理该操作点

所述机器人跟踪过程坐标计算包括开始跟踪、跟踪的保持和结束跟踪，分别称为起跟、跟随保持和退跟，起跟与跟随保持两个过程需要指定工件工艺点，这些点在标定工件基准坐标系后已经示教；机器人跟踪的轨迹可以是直线，也可以是圆弧，走直线时指定插补终点，走圆弧时指定中间点和终点。

所述起跟中机器人从静止状态、普通运动状态，进入对当前传送带上工件的跟踪状态，都为起跟；首先，将机器人当前位置p0即机器人基坐标系下，映射到工件实时局部坐标系track中，得到插补起点pstart；坐标转换如公式(2)所示；

其中，pstart为计算的插补起点，p0为基坐标系下的机器人起点，为传送带坐标系到机器人基坐标系的转换关系，为工件实时局部坐标系track到传送带坐标系的转换关系，由视觉检出和传送带实时位移可计算得到；

插补终点pend为示教过的工艺点在工件实时局部坐标系track下的坐标，计算公式如式(3)所示；

其中，pend为计算的插补终点，p1为示教记录的工艺点，即机器人基坐标系下，为工件基准坐标系到机器人基坐标系的转换关系；

当起跟轨迹为圆弧时，还需要圆弧中间点坐标，插补中间点pmid计算如式(4)所示；

其中，pmid为计算的插补中间点，pm为示教记录的圆弧中间点，即机器人基坐标系下，为工件基准坐标系到机器人基坐标系的转换关系；

以直线插补为例，插补起点、终点均在工件实时局部坐标系track中，则起跟的轨迹在工件实时局部坐标系track中表现为：pstart到pend；将pstart和pend输入插补器，即决定从起点到终点的插补方式，输出一系列插补点pint，经过式(5)转换，得到插补点在机器人基坐标系下位姿；

其中，pintw为一系列插补点转换到机器人基坐标系下的坐标，该坐标值用于控制电机的运动。

所述跟随保持中当机器人起跟完成，达到与传送带相同的速度后，进入跟随保持状态；在跟随保持状态，机器人既可以与工件同步运动，相对速度为0，完成加热工艺；也可以与工件有相对运动，完成点胶工艺；

与起跟相似，先将机器人当前位置p1映射到工件实时局部坐标系track中，得到插补起点pstart，再把示教记录的工艺点p2转换到工件实时局部坐标系track中，得到插补终点pend，走圆弧跟随时增加一个中间点pmid；由于工件上的工艺点相对于工件局部坐标系总是固定的，因此计算track中的插补起点、终点、中间点时，把示教的工艺点转换到工件基准坐标系base下即可，插补起点、终点、中间点的计算如式(6)所示；

其中，pstart,pend,pmid分别为计算的插补中间点，p1p2pm分别为记录的示教点，即机器人基坐标系下，为工件基准坐标系到机器人基坐标系的转换关系；

然后，将pstart和pend(圆弧加上pmid)输入插补器，输出一系列插补点pint，经过式(5)转换，得到插补点在机器人基坐标系下位姿，该坐标值用于控制电机的运动；

利用工件实时局部坐标系将插补点转换到机器人基坐标系，使得任意两个相邻插补点之间的位移包含了传送带单个周期的位移；在跟随保持的过程中，机器人在传送带运动方向的速度分量为滤波后的传送带速度。

所述退跟中，当机器人跟随的工件处理完，机器人结束跟踪状态，从跟踪运动停止、从跟踪运动转为普通运动，均称为退跟；起跟与跟随保持两个过程需要指定工件工艺点，而退跟不需要；

退跟要将处理完的工件从队列中删除，调用cvdown指令，有两种调用方式，跟踪完后直接接普通运动指令，自动结束跟踪状态，然后调用cvdone将工件从队列中删除；也可以跟踪完后先调用cvdone指令，结束跟踪状态，再调用j/l/c指令开始普通运动。

所述退跟后还有跟踪滞后补偿，在机器人完成起跟，保持与工件相对静止时，机器人和传送带速度均为vc，同时停止传送带和机器人，此时测量出机器人末端针尖点与工件上p1点之间的距离δ，即为跟踪滞后量，跟踪滞后补偿时间δt计算如式(7)所示；

δt＝δ/vc(7)

在跟踪过程中，计算工件实时位姿时，工件在传送带坐标系中的x坐标需要加上补偿值，补偿值根据当前传送带速度和跟踪滞后补偿时间δt计算得。

起跟时和退跟后的单个周期内进行基于速度补偿的轨迹优化：在起跟时，对机器人运行速度进行补偿，跟上传送带速度前，速度补偿为负，机器人实际运行滞后于插补点，总滞后量为-s；跟上传送带速度后，速度补偿为正，将滞后量补回来；

同理，退跟时，跟踪结束前一刻，机器人在传送带运动方向仍有速度分量，跟踪结束后的一个周期内，该分量变为0，速度的急剧变化会带来机械冲击；理想的情况是机器人该速度分量从传送带速度逐渐减小，最后变为0对此，从跟踪运动结束的瞬间开始，对机器人运行速度进行补偿，在减速完成前，速度补偿为正，机器人实际运动超前于插补点，总超前量为+s；减速完成后，速度补偿为负，将超前量消去。

本发明的有益效果在于：

本发明是一种轨迹动态跟踪实现方法，与现有技术相比，本发明针对动态跟踪工件上不同工艺点的相关技术进行研究，设计了工件基准坐标系建立、标定和工艺点示教的方法；包括开始跟踪、跟踪的保持和结束跟踪，以及各个过程插补点坐标计算方法；对机器人跟踪滞后量进行了分析并提出滞后时间补偿方式；最后，提出了基于速度补偿的轨迹优化方法，以减缓机器人起跟和退跟时的机械冲击。

附图说明

图1是机器人跟踪动作流程图；

图2是工件基准坐标系示意图；

图3是工艺点示教示意图；

图4是直线跟踪与圆弧跟踪指令示意图；

图5是起跟的坐标变换示意图；

图6是跟随保持的坐标变换示意图；

图7是机器人跟踪滞后测量示意图；

图8是起跟的x方向速度补偿示意图；

图9是退跟的x方向速度补偿示意图。

具体实施方式

下面结合附图对本发明作进一步说明：

本实施例将论述机器人跟踪运动工件上不同工艺点的方法，详细分析机器人跟踪运动过程并阐述插补点坐标计算方法，最后对跟踪效果进行一定优化。根据动态控制策略，当有锁定工件时，机器人开始跟踪动作，首先跟上传送带速度与工件保持相对静止，然后对运动工件进行抓取、涂胶等工艺，处理完后结束跟踪，将工件对象从队列中删除。机器人跟踪动作流程如图1所示。

针对位姿实时变化的不同工艺点的跟踪问题，需要进行工件基准坐标系标定和工艺点示教；机器人跟踪的动作可分为三个过程：开始跟踪、跟踪的保持和结束跟踪，各个过程机器人运动插补坐标计算方式不同；跟踪效果的优化有两方面，一是对机器人跟踪滞后进行补偿，二是解决机器人在开始跟踪和结束跟踪时的机械冲击。

工件建模及标定

对工件进行跟踪操作时有两种情况，一种是只需要跟踪工件中心，这种情况较简单，视觉输出工件中心像素坐标(u,v,θ)，通过标定出的和转换到机器人坐标系下即可实行工件中心跟踪；另一种需要跟踪工件上的非中心点，此时需要建立工件基准坐标系，并示教需要跟踪的工艺点，之后可将机器人操作点映射到工件实时局部坐标系实现任意位姿、任意工艺点的实时坐标计算。

工件基准坐标系建立及标定

在工件上建立基准坐标系base如图2所示，每种工件只需建立一个基准坐标系，使用相同的标定参数，不同种工件需要不同的基准坐标系。

标定时，先将工件放在相机视野内，拍照并根据转换计算得工件的起始位姿p0(传送带坐标系下)，记录此时的编码器读数e1，然后开启传送带使工件运动到机器人工作范围内，记录此时的编码器读数e2，将工件此时的位姿转换到机器人基坐标系下，即为工件基准坐标系base的欧拉角表示，计算如公式(1)所示。

为了后述方便进行坐标变换的表达，将工件基准坐标系到机器人基坐标系的转换关系记为标定的工件基准坐标系相对于机器人基坐标系静止，但是实时的工件局部坐标系track是动态的，由视觉检出和传送带实时位移速度可计算得到。

工艺点示教

工件在传送带上运动时，各个工件的位姿是不同的，同一工件的位姿也是实时变化的，但工艺点在工件实时局部坐标系中的坐标是固定的。通过示教工艺点和工件基准坐标系的关系，可以在跟踪时计算运动工件上不同工艺点的实时坐标。如图3，以示教工件轮廓点为例。

标定完基准坐标系后，移动机器人使tcp对准工件轮廓点，分别记录p1p2p3p4在机器人基坐标系下的坐标，完成工艺点的示教。在跟踪该工艺点时，将该坐标映射到运动工件的实时局部坐标系上，机器人即可处理该操作点。

要求工件基准坐标系标定完成时，不得再启动传送带，不得移动工件位置，直接进行工艺点的示教，否则，需要重新标定工件基准坐标系。

机器人跟踪过程坐标计算

机器人跟踪动作可分为三个过程：开始跟踪、跟踪的保持和结束跟踪，分别称为起跟、跟随保持和退跟，起跟与跟随保持两个过程需要指定工件工艺点，这些点在标定工件基准坐标系后已经示教。机器人跟踪的轨迹可以是直线，也可以是圆弧，走直线时指定插补终点，走圆弧时指定中间点和终点，直线跟踪和圆弧跟踪的指令分别如图4(a)(b)所示。

起跟:

机器人从静止状态、普通运动状态，进入对当前传送带上工件的跟踪状态，都称为起跟。起跟的坐标变换过程如图5所示。

首先，将机器人当前位置p0(机器人基坐标系下)映射到工件实时局部坐标系track中，得到插补起点pstart。坐标转换如公式(2)所示。

其中，pstart为计算的插补起点，p0为基坐标系下的机器人起点，为传送带坐标系到机器人基坐标系的转换关系，为工件实时局部坐标系track到传送带坐标系的转换关系，由视觉检出和传送带实时位移可计算得到。

插补终点pend为示教过的工艺点在工件实时局部坐标系track下的坐标，计算公式如式(3)所示。

其中，pend为计算的插补终点，p1为示教记录的工艺点(机器人基坐标系下)，为工件基准坐标系到机器人基坐标系的转换关系。

当起跟轨迹为圆弧时，还需要圆弧中间点坐标，插补中间点pmid计算如式(4)所示。

其中，pmid为计算的插补中间点，pm为示教记录的圆弧中间点(机器人基坐标系下)，为工件基准坐标系到机器人基坐标系的转换关系。

以直线插补为例，插补起点、终点均在工件实时局部坐标系track中，则起跟的轨迹在工件实时局部坐标系track中表现为：pstart到pend。将pstart和pend输入插补器(决定从起点到终点的插补方式)，输出一系列插补点pint，经过式(5-5)转换，得到插补点在机器人基坐标系下位姿。

其中，pintw为一系列插补点转换到机器人基坐标系下的坐标，该坐标值用于控制电机的运动。

跟随保持

当机器人起跟完成，达到与传送带相同的速度后，进入跟随保持状态。在跟随保持状态，机器人既可以与工件同步运动，相对速度为0，完成加热等工艺；也可以与工件有相对运动，完成点胶等工艺。跟随保持的坐标变换过程如图6所示。

与起跟相似，先将机器人当前位置p1映射到工件实时局部坐标系track中，得到插补起点pstart，再把示教记录的工艺点p2转换到工件实时局部坐标系track中，得到插补终点pend，走圆弧跟随时增加一个中间点pmid。由于工件上的工艺点相对于工件局部坐标系总是固定的，因此计算track中的插补起点、终点、中间点时，把示教的工艺点(机器人基坐标系下)转换到工件基准坐标系base下即可，插补起点、终点、中间点的计算如式(6)所示。

其中，pstart,pend,pmid分别为计算的插补中间点，p1p2pm分别为记录的示教点(机器人基坐标系下)，为工件基准坐标系到机器人基坐标系的转换关系。

然后，将pstart和pend(圆弧加上pmid)输入插补器，输出一系列插补点pint，经过式(5)转换，得到插补点在机器人基坐标系下位姿，该坐标值用于控制电机的运动。

利用工件实时局部坐标系将插补点转换到机器人基坐标系，使得任意两个相邻插补点之间的位移包含了传送带单个周期的位移。在跟随保持的过程中，机器人在传送带运动方向的速度分量为滤波后的传送带速度。

退跟：

当机器人跟随的工件处理完，机器人结束跟踪状态，从跟踪运动停止、从跟踪运动转为普通运动，均称为退跟。起跟与跟随保持两个过程需要指定工件工艺点，而退跟不需要。

退跟要将处理完的工件从队列中删除，调用cvdown指令。有两种调用方式，跟踪完后直接接普通运动指令(如j/l/c)，自动结束跟踪状态，然后调用cvdone将工件从队列中删除；也可以跟踪完后先调用cvdone指令，结束跟踪状态，再调用j/l/c指令开始普通运动。

跟踪滞后补偿：

滞后可分为工件坐标更新滞后、电机响应滞后，最终表现为机器人跟踪滞后。工件坐标更新滞后主要来源于两个方面，一是对编码器读数增量的滑动平均滤波，二是视觉检出时，从相机曝光到机器人控制器解析得到工件像素坐标存在延时；电机响应滞后则是控制系统的特性。

跟踪滞后来源于传感器、数据处理以及传输等的延时，因此用时间量化，设置“跟踪滞后补偿时间”进行补偿，实际滞后距离与传送带速度有关，故不能将跟踪补偿一次性计算在工件初始位姿中，而是需要在跟踪过程中对工件实时坐标进行补偿。

跟踪滞后量的测量如图7所示，机器人跟踪工件上的p1点。

在机器人完成起跟，保持与工件相对静止时，机器人和传送带速度均为vc，同时停止传送带和机器人，此时测量出机器人末端针尖点与工件上p1点之间的距离δ，即为跟踪滞后量，跟踪滞后补偿时间δt计算如式(7)所示。

δt＝δ/vc(7)

在跟踪过程中，计算工件实时位姿时，工件在传送带坐标系中的x坐标需要加上补偿值，补偿值根据当前传送带速度和跟踪滞后补偿时间δt计算得。

基于速度补偿的轨迹优化

在机器人控制器中，插补器以最大加速度进行梯形加减速规划，跟踪时输入插补起、终点，得到插补轨迹。由跟踪坐标的计算可知，利用工件实时局部坐标系将插补点转换到机器人基坐标系，使得任意两个相邻插补点之间的位移包含了传送带单个周期的位移。那么，在起跟时和退跟后的单个周期内，传送带运动方向速度有突变，本节将在速度分析的基础上讨论跟踪轨迹优化的方法。

起跟时，机器人在第一个插补周期就跟上传送带速度，之后，在此基础上继续加速追上目标点。沿传送带运动方向，机器人速度急剧增大，会造成机械冲击。理想的情况是机器人沿传送带运行方向速度从0开始逐渐增大，加速到最大速度运行直到追上目标点，然后减速到传送带速度。对此，设计一种速度补偿方法，如图8所示。

在起跟时，对机器人运行速度进行补偿，跟上传送带速度前，速度补偿为负，机器人实际运行滞后于插补点，总滞后量为-s；跟上传送带速度后，速度补偿为正，将滞后量补回来。

同理，退跟时，跟踪结束前一刻，机器人在传送带运动方向仍有速度分量，跟踪结束后的一个周期内，该分量变为0，速度的急剧变化会带来机械冲击。理想的情况是机器人该速度分量从传送带速度逐渐减小，最后变为0.对此，速度补偿方法如图9所示。

从跟踪运动结束的瞬间开始，对机器人运行速度进行补偿，在减速完成前，速度补偿为正，机器人实际运动超前于插补点，总超前量为+s；减速完成后，速度补偿为负，将超前量消去。

以上显示和描述了本发明的基本原理和主要特征及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。