CN110653826A - 一种面向传送带目标跟随的机器人实时轨迹规划方法 - Google Patents

Abstract

本发明公开了一种面向传送带目标跟随的机器人实时轨迹规划方法，包括：首先得到目标物体在机器人基坐标系下的位姿矩阵；然后根据当前插补时刻的传送带速度预测出下一插补时刻目标物体的位姿矩阵，并计算出机器人的位姿矩阵；再根据运动学逆解计算出下一插补时刻的机器人关节角，并结合当前插补时刻的机器人关节角，得到机器人各关节角度增量，驱动机器人运动；最后计算出机器人与目标物体之间的直线距离L，并将其与容许位移误差em相比较，直到L小于em就判定机器人跟随上目标物体。本发明通过对机器人轨迹进行实时规划，能够解决因传送带速度实时变化而造成的跟随精度不准的问题，对传送带目标物体动态抓取、喷涂以及涂胶等领域具有重要意义。

Description

一种面向传送带目标跟随的机器人实时轨迹规划方法

技术领域

本发明涉及机器人运动控制领域，特别涉及一种面向传送带目标跟随的机器人实时轨迹规划方法。

背景技术

在实际生产线上，通常需要运用机器人对传送带上的目标物体进行诸如拾取、喷涂以及涂胶等操作。对传送带上目标物体进行动态跟随，首要解决的问题就是如何合理的规划机器人跟随轨迹。由于目标物体位置和速度均实时变化，动态轨迹规划不同于机器人传统的示教再现模式，需要根据机器人当前位姿以及下一插补时刻目标物体的位姿实时计算机器人的最优速度，并以最佳运动效果在较短时间内跟随上目标物体。

由于传送带在实际运行过程中并非匀速运动，其速度存在无规律的波动性，为了消除这种不稳定性，需要在一定的采样周期内通过对编码器数值进行处理重新计算传送带的速度，此能提高目标物体的位置预测精度。对于机器人跟随过程中的运动而言，为了保证机器人能够高速平滑运动，需要根据速度和加速度约束条件对其进行实时加减速规划，从而获取任意插补时刻的插补点信息。由于现有的目标物体跟随技术未考虑实际工程中传送带速度存在波动性的问题，因此需要运用一种面向传送带目标跟随的机器人实时轨迹规划方法对跟随轨迹的精度进行精确控制。

发明内容

本发明的目的在于克服现有技术存在的上述不足，提出一种面向传送带目标跟随的机器人实时轨迹规划方法，通过实时更新传送带的速度，更加精确的预测目标物体的位姿，并根据速度和加速度约束条件，对机器人进行实时加减速规划，进而计算出机器人跟随过程中的任意插补时刻的插补点位置信息，获得机器人高速平滑的跟随轨迹，并最终实现机器人快速的跟随上目标物体。本发明可以应用于传送带目标物体动态抓取、喷涂以及涂胶等领域。

为了实现上述发明目的，本发明提供了以下技术方案：

一种面向传送带目标跟随的机器人实时轨迹规划方法，包括以下步骤：

步骤一：将目标物体相对于传送带坐标系下的位姿矩阵，通过齐次坐标变换，得到目标物体在机器人基坐标系下的位姿矩阵；

步骤二：根据当前插补时刻的传送带速度预测出下一插补时刻目标物体相对于传送带坐标系下的位姿矩阵，计算出机器人当前插补位移，得出下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵；

步骤三：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵通过运动学逆解计算出下一插补时刻的机器人关节角，再结合当前插补时刻的机器人关节角，得到机器人各关节角度增量之后，驱动机器人运动；

步骤四：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵和目标物体在机器人基坐标系下的位姿矩阵，计算出两者之间的直线距离L；

步骤五：将所述的L与用户设定的容许位移误差em相比较，判断是否跟随上，若所述的L小于em就判定机器人跟随上目标物体，否则返回步骤二。

进一步的，所述的步骤一中，过程如下：

令目标物体在传送带坐标系下的位姿为(x y z r_x r_y r_z)^T，通过欧拉角变换，获得目标物体在传送带坐标系下的位姿矩阵为^convM_obj，令传送带坐标系相对于机器人基坐标系的齐次变换矩阵为^baseM_conv，通过齐次坐标变换，得到目标物体在机器人基坐标系下的位姿矩阵^baseM_obj如下：

^baseM_obj＝^baseM_conv*^convM_obj。

进一步的，所述的步骤二中，过程如下：

步骤1：计算当前插补时刻的传送带速度v_conv；

令计算周期为F_e个单位采样周期，采样初始时刻编码器值为e_init，采样结束时刻编码器值为e_end，那么传送带速度计算如下：

其中，f_s为传送带与编码器的比例因子，T_sample为单位采样周期对应的时间；

步骤2：根据传送带在单位插补周期内的位移，预测出下一插补时刻目标物体在机器人基坐标系下的位姿矩阵；

令当前插补时刻目标物体在机器人基坐标系下的位姿矩阵为^baseM_obj，传送带在单位插补周期内的位移为△P_conv＝(△x △y △z 0)^T，其中△x，△y和△z分别为目标物体在传送带坐标系X，Y和Z轴正方向上的位移量，那么下一插补时刻目标物体在机器人基坐标系下的位姿矩阵计算如下：

其中，

步骤3：根据当前插补时刻的机器人位姿矩阵M_robot和所述的下一插补时刻目标物体在机器人基坐标系下的位姿矩阵，计算出当前插补时刻机器人的插补位移△s；

当前插补时刻的机器人与下一插补时刻的目标物体之间的距离如下：

l＝||^baseP′_obj-P_robot||

其中，矢量^baseP′_obj、P_robot分别为矩阵^baseM′_obj和M_robot的位置矢量；

令机器人最大速度为v_m，最大加速度为a_m，机器人单位插补周期为△T，当前插补时刻速度和加速度分别为v_c和a_c，实时梯型加减速的匀加速段、匀速段和匀减速段中的速度和位移约束条件如下；

(4)匀加速段

机器人进入匀加速段需要满足的速度和位移约束条件如下：

其中：

l_a表示加速段位移，l_d表示减速段位移；

(5)匀速段

机器人进入匀速段需要满足的位移约束条件如下：

(6)匀减速段

机器人进入匀减速段需要满足的位移约束条件如下：

|v_c△T-0.5a_m△T²|≤l

通过上述约束条件，确定出机器人的运动状态，当前插补时刻机器人的插补位移△s为：

△s＝v_c△T+0.5a_c△T²

步骤4：根据当前插补时刻的插补位移，计算出机器人当前插补时刻的姿态转角θ_cur；

令机器人上一插补时刻的姿态转角为θ_a，根据步骤一所述的目标物体在机器人基坐标系下的位姿矩阵和初始状态下的机器人位姿矩阵计算出等效转轴矢量f_axis和等效位姿转角θ_f以及两者之间的距离l_init，那么机器人当前插补时刻的姿态转角θ_cur计算如下：

步骤5：根据当前插补时刻的机器人位姿矩阵、插补位移以及姿态转角计算出下一插补时刻机器人的位姿矩阵；

首先根据当前插补时刻的机器人位置矢量和下一插补时刻目标物体的位置矢量计算出两者之间的方向矢量如下：

下一插补时刻机器人的位姿矩阵计算如下：

其中，R_robot为矩阵M_robot的姿态部分，R_cur为根据等效转轴矢量f_axis和当前插补时刻的姿态转角θ_cur计算出来的姿态旋转矩阵。

进一步的，所述的步骤三中，过程如下：

步骤1：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵通过运动学逆解计算出下一插补时刻的机器人关节角；

步骤2：根据下一插补时刻的机器人关节角和当前插补时刻的机器人关节角，得到机器人各关节角度增量之后，驱动机器人运动。

进一步的，所述的步骤四中，过程如下：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵和目标物体在机器人基坐标系下的位姿矩阵，计算出两者之间的直线距离L。

进一步的，所述的步骤五中，过程如下：将所述的L与用户设定的容许位移误差em相比较，判断是否跟随上，若所述的L小于em就判定机器人跟随上目标物体，否则返回步骤二。

本发明提出的面向传送带目标跟随的机器人实时轨迹规划方法，机器人跟随上目标物体的判断条件如下：若步骤四中所述的下一插补时刻机器人的位姿矩阵和目标物体之间的直线距离小于用户设定的容许位移误差，那么就判定机器人跟随上目标物体。

与现有技术相比，本发明的有益效果：

1、本发明通过在一定的采样周期内对编码器数值进行处理，重新计算传送带的速度，从而预测出目标物体的位姿，此有效地避免了传送带速度不规律的波动性，提高了目标物体的位置预测精度；

2、本发明考虑机器人速度和加速度约束条件，能够实时获得机器人最佳加减速状态；

3、本发明所提出的实时加减速控制方法能够应用于插补轨迹实时变化场合，并能够获得高速平滑的跟随轨迹；

4、本发明的传送带目标跟随技术能够适用于任意构型的机器人，并且可以应用于传送带目标物体动态抓取、喷涂以及涂胶等领域。

附图说明

图1是本发明方法的整体流程图；

图2是本发明传送带速度计算的流程图；

图3是本发明实时加减速规划流程图；

图4是本发明的机器人跟随空间轨迹曲线图；

图5是本发明的机器人跟随速度曲线图。

具体实施方式

下面结合试验例及具体实施方式对本发明作进一步的详细描述。但不应将此理解为本发明上述主题的范围仅限于以下的实施例，凡基于本发明内容所实现的技术均属于本发明的范围

结合附图对本发明的面向传送带目标跟随的机器人实时轨迹规划方法进行详细说明。

如图1所示，其为本发明的面向传送带目标跟随的机器人实时轨迹规划方法的整体流程图，具体步骤如下：

步骤一：将目标物体相对于传送带坐标系下的位姿矩阵，通过齐次坐标变换，得到目标物体在机器人基坐标系下的位姿矩阵；

令目标物体在传送带坐标系下的位姿为(x y z r_x r_y r_z)^T，通过欧拉角变换，获得目标物体在传送带坐标系下的位姿矩阵为^convM_obj，令传送带坐标系相对于机器人基坐标系的齐次变换矩阵为^baseM_conv，通过齐次坐标变换，得到目标物体在机器人基坐标系下的位姿矩阵^baseM_obj如下：

^baseM_obj＝^baseM_conv*^convM_obj

步骤二：根据当前插补时刻的传送带速度预测出下一插补时刻目标物体相对于传送带坐标系下的位姿矩阵，计算出机器人当前插补位移，得出下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵；

步骤二包含如下步骤：

步骤1：计算当前插补时刻的传送带速度v_conv；

如图2所示，其为传送带速度计算整体流程图。令计算周期为F_e个单位采样周期，采样初始时刻编码器值为e_init，采样结束时刻编码器值为e_end，那么传送带速度计算如下：

其中，f_s为传送带与编码器的比例因子，T_sample为单位采样周期对应的时间；

步骤2：根据传送带在单位插补周期内的位移，预测出下一插补时刻目标物体在机器人基坐标系下的位姿矩阵；

令当前插补时刻目标物体在机器人基坐标系下的位姿矩阵为^baseM_obj，传送带在单位插补周期内的位移为△P_conv＝(△x △y △z 0)^T，其中△x，△y和△z分别为目标物体在传送带坐标系X，Y和Z轴正方向上的位移量，那么下一插补时刻目标物体在机器人基坐标系下的位姿矩阵计算如下：

其中，

步骤3：根据当前插补时刻的机器人位姿矩阵M_robot和所述的下一插补时刻目标物体在机器人基坐标系下的位姿矩阵，计算出当前插补时刻机器人的插补位移△s；

当前插补时刻的机器人与下一插补时刻的目标物体之间的距离如下：

l＝||^baseP′_obj-P_robot||

其中，矢量^baseP′_obj、P_robot分别为矩阵^baseM′_obj和M_robot的位置矢量；

令机器人最大速度为v_m，最大加速度为a_m，机器人单位插补周期为△T，当前插补时刻速度和加速度分别为v_c和a_c，实时梯型加减速的匀加速段、匀速段和匀减速段中的速度和位移约束条件如下；

(7)匀加速段

机器人进入匀加速段需要满足的速度和位移约束条件如下：

其中：

l_a表示加速段位移，l_d表示减速段位移；

(8)匀速段

机器人进入匀速段需要满足的位移约束条件如下：

(9)匀减速段

机器人进入匀减速段需要满足的位移约束条件如下：

|v_c△T-0.5a_m△T²|≤l

通过上述约束条件，确定出机器人的运动状态，当前插补时刻机器人的插补位移△s为：

△s＝v_c△T+0.5a_c△T²

步骤4：根据当前插补时刻的插补位移，计算出机器人当前插补时刻的姿态转角θ_cur；

令机器人上一插补时刻的姿态转角为θ_a，根据步骤一所述的目标物体在机器人基坐标系下的位姿矩阵和初始状态下的机器人位姿矩阵计算出等效转轴矢量f_axis和等效位姿转角θ_f以及两者之间的距离l_init，那么机器人当前插补时刻的姿态转角θ_cur计算如下：

步骤5：根据当前插补时刻的机器人位姿矩阵、插补位移以及姿态转角计算出下一插补时刻机器人的位姿矩阵；

首先根据当前插补时刻的机器人位置矢量和下一插补时刻目标物体的位置矢量计算出两者之间的方向矢量如下：

下一插补时刻机器人的位姿矩阵计算如下：

其中，R_robot为矩阵M_robot的姿态部分，R_cur为根据等效转轴矢量f_axis和当前插补时刻的姿态转角θ_cur计算出来的姿态旋转矩阵。

步骤三：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵通过运动学逆解计算出下一插补时刻的机器人关节角，再结合当前插补时刻的机器人关节角，得到机器人各关节角度增量之后，驱动机器人运动；

步骤三包括如下步骤：

步骤1：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵通过运动学逆解计算出下一插补时刻的机器人关节角；

步骤2：根据下一插补时刻的机器人关节角和当前插补时刻的机器人关节角，得到机器人各关节角度增量之后，驱动机器人运动。

步骤四：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵和目标物体在机器人基坐标系下的位姿矩阵，计算出两者之间的直线距离L；

步骤五：将所述的L与用户设定的容许位移误差em相比较，判断是否跟随上，若所述的L小于em就判定机器人跟随上目标物体，否则返回步骤二。

本发明提出的面向传送带目标跟随的机器人实时轨迹规划方法，机器人跟随上目标物体的判断条件如下：若步骤四中所述的下一插补时刻机器人的位姿矩阵和目标物体之间的直线距离小于用户设定的容许位移误差，那么就判定机器人跟随上目标物体。

为验证本发明所提的面向传送带目标跟随的机器人实时轨迹规划方法的有效性，现给出实验验证过程。

以直线型传送带目标跟随为例，令开始跟随时刻目标物体在传送带坐标系的位姿向量为(57.789070 84.626152 0 0 0 π/6.0)^T，传送带坐标系相对于机器人基坐标系的齐次变换矩阵为：

那么通过齐次坐标变换，得到目标物体在机器人基坐标系下的位姿矩阵^baseM_obj。机器人在开始跟随时刻的位姿矩阵为：

传送带的速度为125mm/s，机器人最大指令速度v_m为600mm/s。那么经过本发明提出的方法，经过826ms后机器人就可跟随上传送带上的目标物体，跟随响应快，并且如图4和5所示，机器人跟随轨迹曲线光顺平滑，且从速度曲线可知，机器人速度能够达到最大指令速度，且其匀速阶段为396ms，占总体跟随时间的47.941889％，说明跟随效率较高。

综上所述，本发明所提出的面向传送带目标跟随的机器人实时轨迹规划方法可以保证机器人在较短时间内跟随上传送带上的目标物体，并且机器人跟随轨迹曲线光顺平滑，跟随效率高，可适用于任意构型的机器人，并可应用于传送带目标物体动态抓取、喷涂以及涂胶等领域。

Claims (3)

1.一种面向传送带目标跟随的机器人实时轨迹规划方法，其特征在于，包括以下步骤：

步骤一：将目标物体相对于传送带坐标系下的位姿矩阵，通过齐次坐标变换，得到目标物体在机器人基坐标系下的位姿矩阵；

步骤二：根据当前插补时刻的传送带速度预测出下一插补时刻目标物体相对于传送带坐标系下的位姿矩阵，计算出机器人当前插补位移，得出下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵；

步骤三：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵通过运动学逆解计算出下一插补时刻的机器人关节角，再结合当前插补时刻的机器人关节角，得到机器人各关节角度增量之后，驱动机器人运动；

步骤四：根据所述的下一插补时刻机器人末端执行器相对于机器人基坐标系的位姿矩阵和目标物体在机器人基坐标系下的位姿矩阵，计算出两者之间的直线距离L；

步骤五：将所述的L与用户设定的容许位移误差em相比较，判断是否跟随上，若所述的L小于em就判定机器人跟随上目标物体，否则返回步骤二。

2.根据权利要求1所述的面向传送带目标跟随的机器人实时轨迹规划方法，其特征在于，所述的步骤一中，所述齐次坐标变换过程如下：

令目标物体在传送带坐标系下的位姿为(x y z r_x r_y r_z)^T，通过欧拉角变换，获得目标物体在传送带坐标系下的位姿矩阵为^convM_obj，令传送带坐标系相对于机器人基坐标系的齐次变换矩阵为^baseM_conv，通过齐次坐标变换，得到目标物体在机器人基坐标系下的位姿矩阵^baseM_obj如下：

^baseM_obj＝^baseM_conv*^convM_obj。

3.根据权利要求2所述的面向传送带目标跟随的机器人实时轨迹规划方法，其特征在于，所述步骤二包括如下步骤：

步骤1：计算当前插补时刻的传送带速度v_conv；

令计算周期为F_e个单位采样周期，采样初始时刻编码器值为e_init，采样结束时刻编码器值为e_end，那么传送带速度计算如下：

其中，f_s为传送带与编码器的比例因子，T_sample为单位采样周期对应的时间；

步骤2：根据传送带在单位插补周期内的位移，预测出下一插补时刻目标物体在机器人基坐标系下的位姿矩阵；

令当前插补时刻目标物体在机器人基坐标系下的位姿矩阵为^baseM_obj，传送带在单位插补周期内的位移为△P_conv＝(△x △y △z 0)^T，其中△x，△y和△z分别为目标物体在传送带坐标系X，Y和Z轴正方向上的位移量，那么下一插补时刻目标物体在机器人基坐标系下的位姿矩阵计算如下：

其中，

步骤3：根据当前插补时刻的机器人位姿矩阵M_robot和所述的下一插补时刻目标物体在机器人基坐标系下的位姿矩阵，计算出当前插补时刻机器人的插补位移△s；

当前插补时刻的机器人与下一插补时刻的目标物体之间的距离如下：

l＝||^baseP′_obj-P_robot||

其中，矢量^baseP′_obj、P_robot分别为矩阵^baseM′_obj和M_robot的位置矢量；

令机器人最大速度为v_m，最大加速度为a_m，机器人单位插补周期为△T，当前插补时刻速度和加速度分别为v_c和a_c，实时梯型加减速的匀加速段、匀速段和匀减速段中的速度和位移约束条件如下；

(1)匀加速段

机器人进入匀加速段需要满足的速度和位移约束条件如下：

其中：

l_a表示加速段位移，l_d表示减速段位移；

(2)匀速段

机器人进入匀速段需要满足的位移约束条件如下：

(3)匀减速段

机器人进入匀减速段需要满足的位移约束条件如下：

|v_c△T-0.5a_m△T²|≤l

通过上述约束条件，确定出机器人的运动状态，当前插补时刻机器人的插补位移△s为：

△s＝v_c△T+0.5a_c△T²

步骤4：根据当前插补时刻的插补位移，计算出机器人当前插补时刻的姿态转角θ_cur；

令机器人上一插补时刻的姿态转角为θ_a，根据步骤一所述的目标物体在机器人基坐标系下的位姿矩阵和初始状态下的机器人位姿矩阵计算出等效转轴矢量f_axis和等效位姿转角θ_f以及两者之间的距离l_init，那么机器人当前插补时刻的姿态转角θ_cur计算如下：

步骤5：根据当前插补时刻的机器人位姿矩阵、插补位移以及姿态转角计算出下一插补时刻机器人的位姿矩阵；

首先根据当前插补时刻的机器人位置矢量和下一插补时刻目标物体的位置矢量计算出两者之间的方向矢量如下：

下一插补时刻机器人的位姿矩阵计算如下：

其中，R_robot为矩阵M_robot的姿态部分，R_cur为根据等效转轴矢量f_axis和当前插补时刻的姿态转角θ_cur计算出来的姿态旋转矩阵。

Images