一种机器人关节空间传送带跟随运动的轨迹规划方法与流程

本发明涉及一种机器人运动轨迹的规划方法，具体说是一种机器人关节空间传送带跟随运动的轨迹规划方法。

背景技术：

机器人的一个常见应用领域是跟踪传送带上的物体，实现如抓取、包装、分拣等应用。

典型的传送带跟随应用环境如附图1所示，包含的装置主要有：机器人、直线传送带、脉冲编码器、视觉系统和工件。传送带上具有位置检测装置脉冲编码器，可检测传送带走过的距离，视觉系统位于传送带上方，使用视觉系统对直线传送带上的工件定位。传送带运行过程中，视觉系统检测处于拍摄区域中的工件，视觉系统将拍摄区域中的工件的位置信息拍摄并记录，将位置信息传递给控制器，直线传送带将工件从上游方向往下游方向进行直线传输，脉冲编码器检测出直线传送带进行移动后所产生的直线距离数据，并将数据传输给机器人，机器人对数据进行分析并计算出所需要的工作轨迹的补偿量，实现准确定位，最后机器人将对工件实施跟随运动，完成相应的如动态抓取等动作。

目前应用较广的跟随方法是基于视觉传感器的目标跟踪方法，通过图像处理等算法实现目标的跟踪，但这种算法受光照影响较大，对图像处理算法要求高，且算法复杂度较高。有些文献则直接对机器人的末端轨迹进行规划，即在直角坐标系下进行规划，这种方法比较直观，跟随轨迹较好，但这种方法易出现在有些轨迹处关节并未达到最大速度，导致跟随效率低，而有些轨迹又可能出现超最大关节速度或最大关节加速度的情况，则须进行特殊处理，较为复杂。

技术实现要素：

本发明所要解决的技术问题，在于克服现有技术存在的缺陷，提出一种机器人关节空间传送带跟随运动的轨迹规划方法，本方法只需要视觉模块定位物体的初始位置，后续跟随过程中无需视觉定位，且算法基于机器人关节空间的规划，直接对机器人各个关节进行轨迹规划，保证各个关节不超最大加速度和最大速度，且可以以最短的时间去跟随目标物体，跟随效率高。本发明目的是使机器人能够快速的跟踪上传送带上的目标物体，并与目标物体同步运动，实现动态跟随，进而完成如抓取等相关应用功能。

本发明所提出的基于关节空间的机器人跟随运动轨迹规划方法，其步骤如下：

(1)获得目标物体的位置和速度矢量：根据视觉模块输出的视觉坐标系下目标物体的当前位置(x_vision,y_vision,z_vision)，通过坐标系转换获得目标物体在直角坐标系下的位置(x_base,y_base,z_base)：

(x_base,y_base,z_base,1)^T＝^baseT_vision·(x_vision,y_vision,z_vision,1)^T

式中，^baseT_vision为视觉坐标系相对于直角坐标系的坐标转换矩阵，坐标转换矩阵是指两坐标系的旋转和平移之间的关系。传送带和视觉系统安装固定后，该转换矩阵可直接获得。

通过编码器实时反馈的脉冲数据，采用差分的方法获得目标物体在直角坐标系下的线速度v_base。

(2)预测目标物体的运动轨迹：目的是让机器人去跟踪目标物体的预测位置而不是当前位置，这样可以避免出现机器人无限接近于目标物体而却始终跟随不上的现象。预测位置的实质是预测时间，即机器人不去跟踪目标物体的当前位置(x_vision,y_vision,z_vision)＝(x₀,y₀,z₀)(0时刻)，而是去跟踪目标物体t时刻所在的位置(x_t,y_t,z_t)。

时间t即为预测时间。本发明根据之前的目标物体位置信息和当前的位置信息，拟合出传送带的运动规律，因为短时间内传送带的变化是相对均匀的，因此拟合出的传送带运动规律为匀速运动或匀加速运动，即得到传送带的运动速度v_convery和加速度a_convery，再根据机器人与目标物体当前的距离Δs，以及二者的速度矢量差ΔV，建立相应的预测模型，预测出时间t。预测模型的总体思想是当机器人与目标物体离得越近，速度矢量差得越少，预测的时间越小，反之越大。

t＝f(Δs,ΔV)

(3)逆解至关节空间：根据目标物体在机器人直角坐标系下的位置(x_base,y_base,z_base)和速度矢量v_base，将目标物体位置逆解至关节空间。位置逆解通过运动学逆解模型，即可得到各个关节的目标位置q；采用雅克比矩阵的方法，得到各个关节目标速度矢量

式中，J^-1(q)为雅克比矩阵的逆矩阵；V为笛卡尔速度，笛卡尔速度是直角坐标系下的线速度v_base和角速度的组合，本发明中设定目标物体在传送带运动过程中不发生旋转，故角速度为0。

(4)关节轨迹规划：根据关节初始的位置q₀和速度矢量以及通过(3)得到的目标位置q和目标速度矢量在关节空间内进行关节轨迹规划。关节轨迹规划的方法和具体步骤如下：

①各关节以梯形加减速进行轨迹规划：根据关节位移(q-q₀)和初速度矢量末速度矢量规划出速度曲线，以最大加速度进行加减速，得出各关节规划的最短时间T_imin。

②判定各关节该速度规划下是否存在死区，若存在，给出死区的左右边界T_left、T_right。死区是针对运行时间而言的，在保证运动约束(最大速度、最大加速度)条件下，从初始状态(位置、速度)达到目标状态(位置、速度)最小时间是存在的。但在最小时间的基础上继续增大运行时间，有一段时间范围是无法规划出满足始末状态的速度曲线来，这段时间称为“死区”。

③以max{T_imin,i＝1…n}(n为关节数)为基准时间，若该时间在某一关节的死区内，则以该关节死区的右边界T_right为轨迹规划的基准时间，依次判断各关节，最终得到跨越了各关节死区的最优的轨迹规划时间T_min。

④以③得到的最优的轨迹规划时间T_min为轨迹规划时间，拓展其他关节的轨迹规划时间，重新对各个关节轨迹进行规划，轨迹规划的原则是在原轨迹规划的基础上保证尽可能多的匀速段。

(5)根据(4)的关节轨迹规划，实时输出各关节的目标位置，再通过脉冲输出模块，转换为各个关节对应的电机脉冲值，驱动机器人运动至目标位置，进而实现实时动态地跟踪目标物体，达到跟随的效果。当机器人跟随上目标物体，便可以实现抓取等相应的动作。

本发明提出的基于关节空间的传送带跟随运动轨迹规划方法，仅需视觉系统给出目标物体的初始位置，后续跟随不再需要物体定位，所需视觉算法简单。且该方法是在关节空间内对机器人进行动态实时规划，可以实现在约束条件下时间最优的动态轨迹规划，可以保证机器人以最短的时间跟踪上目标物体。

本发明所提出的基于关节空间的轨迹规划，直接对关节轨迹进行规划，可以保证各关节不超最大速度和最大加速度，若直接在直角坐标系下进行轨迹规划，很容易出现关节超速的情况，这就必须进行超速处理，相较于关节空间的规划较为复杂；或出现关节达不到最大速度的情况，使得跟随不够快，相较于关节空间的规划效率较低。

本发明中所提出的轨迹预测方法，可以保证机器人快速跟踪上目标物体，避免出现无限接近目标物体而始终跟不上的情况。且该轨迹预测的方法，可以保证各关节速度曲线平滑，避免由于目标物体不断改变而导致关节速度变化过于频繁，易出现震动。

本发明所提出的关节空间的传送带跟随运动轨迹规划方法，以某一关节为基准轴，拓展其他关节的轨迹规划时间时，考虑了某一时间段内关节规划是无解的情况(死区)，进行了规避，保证了在跟随整个过程，各个关节均可以不超最大加速度和最大速度，并以最快的时间进行轨迹规划。

本发明的关键点在于在关节空间内对机器人进行动态实时规划，可实现在约束条件下时间最优的动态轨迹规划，算法简单，易于实现。本方法对目标物体的运动轨迹进行预测，可避免出现无限接近物体而跟不上的现象，并且保证关节速度平滑。在规划关节轨迹时，规避了关节无解的区域，保证跟随过程各个关节均可以满足约束条件。

本发明提出的关节空间的传送带跟随运动轨迹规划方法，并不只适用于传送带的跟随运动，凡是根据本发明的技术实质对应用场景所作的任何简单修改，结构变化等均属于本发明的保护范围。

附图说明

图1是传送带跟随应用环境示意图。其中，1是机器人，2是视觉系统，3是工件，4是传送带，5是传送带运行方向。

图2是传送带跟随运动流程图。

图3是机器人关节空间轨迹规划原理图。

图4是SCARA机器人结构示意图。

图5是跟随过程机器人末端轨迹与物体运动轨迹图。其中，a是x-y-z三维空间内的运动轨迹，b是xy平面内的运动轨迹，c是xz平面内的运动轨迹，d是yz平面内的运动轨迹，图中粗实线为机器人末端轨迹，细实线为物体运动轨迹。

图6是跟随过程机器人末端位置与姿态曲线图。

图7是跟随过程机器人各关节速度曲线图。

具体实施方式

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

本发明以SCARA机器人(附图4所示)为实验对象，其杆件参数为：L1＝L2＝300cm,L3＝200cm，对其应用本发明所提出的跟随算法。测试环境如附图1所示，传送带安装在直角坐标系的X轴上，匀速运动，速度为133.33mm/s。

整个跟随流程如附图2为：当视觉模块检测到目标物体时，程序判断是否满足跟随条件，若满足则动态跟随，所谓动态跟随指的是每4ms(控制系统的插补时间)读取一次目标物体的位置并进行预测，机器人朝预测的位置运动，跟随过程中，机器人末端点与目标物体在Z方向上相距40mm，当跟随到目标物体时，收到抓取指令时，向下抓取物体，抓取到目标物体后，向Z方向运动100mm，再执行下个目标物体的跟随抓取动作。

附图5为跟随过程机器人末端点运动轨迹和物体的运动轨迹图。粗实线为机器人末端轨迹，细实线为物体运动轨迹，两直线相交代表跟随上物体，之后在xy平面两者运动轨迹相同，当抓取到目标物体后在yz、xz平面两者运动轨迹相同。

附图6为跟随过程机器人末端点的位置(X、Y、Z)和姿态(C)曲线。

附图7为跟随过程各个关节的关节速度曲线，跟随过程各关节速度曲线较为平滑。