[发明专利]一种机器人目标跟踪轨迹规划方法

权利要求书

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与目标物体行进路线之间存在较大位移差，机器人必须从等待点运动到目标物体行进路线上才可与之同步；

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

根据垂直方向的位移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速度预规划计算出的时间，对机器人在平行方向或垂直方向的运动进行调整：

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

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

根据以上预测，当TyTx-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的跟踪轨迹。