一种轨迹动态跟踪实现方法技术

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

【技术实现步骤摘要】
一种轨迹动态跟踪实现方法
本专利技术涉及机器人
，尤其涉及一种轨迹动态跟踪实现方法。
技术介绍
在机器人动态作业中，难点在于对运动目标的跟踪。一部分跟踪算法是基于目标的特征检测，建立特征模型，计算复杂且耗时，不适合工业机器人实时、快速抓取的要求。一部分是基于滤波理论的跟踪算法，例如卡尔曼滤波算法，利用线性系统状态方程，根据系统输入对系统状态进行最优估计，更新系统估计状态并不断迭代，它可以较为准确的预测目标的运动状态从而实现跟踪；后续研究者针对非线性系统提出了扩展卡尔曼滤波算法，但其涉及矩阵求导，对处理器而言运算复杂。邹腾跃等提出了基于背景建模的目标检测和基于局部稀疏表达的粒子滤波跟踪方法，并采用反馈机制实现了变焦跟踪，其算法具有较高的准确性和鲁棒性，但牺牲了一定的计算时间。邓明星等提出了改进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；跟上传送带本文档来自技高网...

【技术保护点】
1.一种轨迹动态跟踪实现方法，其特征在于：包括工件建模及标定和机器人跟踪过程坐标计算，所述工件建模及标定包括工件基准坐标系建立及标定、工艺点示教，所述工件基准坐标系建立及标定在标定时，先将工件放在相机视野内，拍照并根据

【技术特征摘要】
1.一种轨迹动态跟踪实现方法，其特征在于：包括工件建模及标定和机器人跟踪过程坐标计算，所述工件建模及标定包括工件基准坐标系建立及标定、工艺点示教，所述工件基准坐标系建立及标定在标定时，先将工件放在相机视野内，拍照并根据转换计算得工件的起始位姿P0即传送带坐标系下，记录此时的编码器读数E1，然后开启传送带使工件运动到机器人工作范围内，记录此时的编码器读数E2，将工件此时的位姿转换到机器人基坐标系下，即为工件基准坐标系Base的欧拉角表示，计算如公式(1)所示：



为了后述方便进行坐标变换的表达，将工件基准坐标系到机器人基坐标系的转换关系记为标定的工件基准坐标系相对于机器人基坐标系静止，但是实时的工件局部坐标系Track是动态的，由视觉检出和传送带实时位移速度可计算得到；
所述工艺点示教：通过示教工艺点和工件基准坐标系的关系，可以在跟踪时计算运动工件上不同工艺点的实时坐标；标定完基准坐标系后，移动机器人使TCP对准工件轮廓点，分别记录P1P2P3P4在机器人基坐标系下的坐标，完成工艺点的示教；在跟踪该工艺点时，将该坐标映射到运动工件的实时局部坐标系上，机器人即可处理该操作点
所述机器人跟踪过程坐标计算包括开始跟踪、跟踪的保持和结束跟踪，分别称为起跟、跟随保持和退跟，起跟与跟随保持两个过程需要指定工件工艺点，这些点在标定工件基准坐标系后已经示教；机器人跟踪的轨迹可以是直线，也可以是圆弧，走直线时指定插补终点，走圆弧时指定中间点和终点。


2.根据权利要求1所述的轨迹动态跟踪实现方法，其特征在于：所述起跟中机器人从静止状态、普通运动状态，进入对当前传送带上工件的跟踪状态，都为起跟；首先，将机器人当前位置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为一系列插补点转换到机器人基坐标系下的坐标，该坐标值用于控制电机的运动。


3.根据权利要求1所述的轨迹动态跟踪实现方法，其特征...

【专利技术属性】
技术研发人员：宋宝，周向东，唐小琦，徐迪炜，余文涛，李虎，杨承博，谢正莹，
申请(专利权)人：广东拓斯达科技股份有限公司，华中科技大学，
类型：发明
国别省市：广东;44