CN113858213A - 一种用于目标同步的机器人动态轨迹规划方法 - Google Patents

Abstract

本发明公开了一种用于目标同步的机器人动态轨迹规划方法，涉及机器人控制方法领域。使机器人能够从静止状态平稳启动追踪运动中的目标物体，并最终与其保持同步。步骤1、参数定义及轨迹参数给定；步骤2、划分阶段；步骤3、进行加速同步阶段的动态规划；步骤4、进行减速同步阶段的动态规划；步骤5、完成追踪进入同步阶段；整个规划过程运算量小，能够满足实时插补计算的要求，且无需预测目标物体的位置，能够保证一定的同步精度。在追踪过程中具备安全校验，可以保证实际运行过程中设备的安全性。当机器人末端与目标物体实现同步后，可进一步根据实际应用中的工艺需求执行复杂的轨迹运动。

Description

一种用于目标同步的机器人动态轨迹规划方法

技术领域

本发明涉及机器人控制方法领域。

背景技术

机器人目前已广泛应用于自动化作业中，其中较为常见的一种应用是追踪传送带上的目标物体并与其保持同步，从而完成诸如抓取、包装、分拣、涂胶、在线检测等复杂工艺。由于运动中的目标物体具有位置不确定性，因此能够动态规划机器人的运动轨迹，使其实时追踪目标物体，保持一定的同步精度并且整个运动过程平滑且顺畅，具有实际工程意义。

在现有技术中，文献《机器人操作臂跟踪动态目标的轨迹规划方法》提出了一种基于遗传算法的动态轨迹规划方法，得到了机器人操作臂跟踪目标物体的运动轨迹，然而遗传算法本身的时间复杂性较高，在机器人轨迹规划中难以保证实时性。名为“一种机器人关节空间传送带跟随运动的轨迹规划方法”、申请号为“201610222488.1”的中国发明专利申请中公开了一种关节空间下的传送带跟随运动的轨迹规划方法，由于该方法是在关节空间下规划的，映射至笛卡尔空间后最终的轨迹不可控，在实际应用中还需另外设计相应的保护机制，否则机器人存在撞机风险。名为“基于S曲线加减速的运动目标动态跟踪方法、系统及装置”、申请号为“201710983709.1”的中国发明专利申请以及名为“一种用于机器人的传送带同步跟踪方法、装置和系统”、申请号为“201810467241.5”的中国发明专利申请中公开的动态跟踪方法均使用了预测汇合点的思想，即事先计算出机器人末端与目标物体的汇合点，基于“预测、规划、执行”的思想完成动态跟踪，然而实际应用中传送带的速度会存在波动，可能导致汇合点预测不准，从而损失一定的精度。名为“一种工业机器人传送带动态跟踪方法”、申请号为“201810467881.6”的中国发明专利申请公开了一种基于机器人末端速度合成思想的动态跟踪方法，通过对合成的末端速度进行积分得到最终的行进路径，该合成速度包含示教速度、实际传送带速度与误差修正速度，由于实际传送带的速度是波动变化的，直接将其合成则无法保证机器人在启动阶段的平稳性。名为“一种基于分解速度规划算法的移动物体抓取方法”、申请号为“201811399335.X”的中国发明专利申请，名为“一种机器人目标跟踪轨迹规划方法”、申请号为“201911347861.6”的中国发明专利申请以及文献《机器人动态物料追随抓取技术的轨迹算法设计》均是基于运动分解的思想实现了对于目标物体的轨迹跟踪，前者是基于速度分解的思想，后两者则是基于位移分解的思想，虽然具备速度收敛快、定位过程平稳的优势，但是当机器人实现跟踪后因工艺要求需同步实现复杂的轨迹运动时，此类方法将不再适用。

发明内容

本发明针对以上问题，提出了一种用于目标同步的机器人动态轨迹规划方法，使机器人能够从静止状态平稳启动追踪运动中的目标物体，并最终与其保持同步。

本发明的技术方案按以下步骤进行：

步骤1、参数定义及轨迹参数给定；

输入用户设置的传送带参考速度Vcvy；输入机器人跟随加速度a_m以及插补周期dt；

定义期望加速a_i、位移同步允差S_ε、速度同步允差V_ε；定义同步阶段跳转系数k_s；定义加速同步阶段实际时间t_a、加速同步阶段参考时间T_a、加速同步阶段影响系数k_a、减速同步阶段实际时间t_d、减速同步阶段参考时间T_d、减速同步阶段影响系数k_d；

步骤2、划分阶段；

在传送带无速度波动的理想情况下，机器人从静止状态开始追踪运动中的目标物体并最终与其保持同步，势必会经历一个先加速至超过目标物体参考速度后再减速至目标物体参考速度的过程，整个同步过程中会出现两次机器人当前速度等于目标物体参考速度的情况，在第一次相等的时刻，机器人当前位置与目标物体位置的位移差应达到最大值，在第二次相等的时刻，机器人当前位置与目标物体位置的位移差应满足位移同步允差要求，此时速度与位置均跟踪到位，即实现同步；基于以上思想，可将整个追踪与同步过程按顺序划分为三个阶段，分别为加速同步阶段、减速同步阶段、同步阶段；

步骤3、进行加速同步阶段的动态规划；

加速同步阶段的主要目的是让机器人平稳启动并缩小与目标物体之间的位移差；本阶段中设置三个同步阶段跳转条件，条件一：当前插补周期中机器人的当前位置Scur_i小于目标位置Strgt_i；条件二：当前插补周期中机器人当前速度Vcur_i大于目标速度Vtrgt_i；条件三：当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i小于机器人当前位置Scur_i和同步阶段跳转系数k_s的乘积；

若当前各项参数同时满足条件一、条件二、条件三，则跳转至步骤4进入减速同步阶段的动态规划，若有一个条件不满足，则先执行插补运动再进行超时校验；

若超时，则机器人结束本次追踪同步运动并退出至安全位置；若执行插补运动后不超时，则重新进行步骤3；

步骤4、进行减速同步阶段的动态规划；

减速同步阶段的主要目的是让机器人的当前位置与速度不断逼近目标物体的位置与速度；本阶段中设置两个同步条件，条件四：当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i小于位移同步允差S_ε；条件五：即当前插补周期中目标速度Vtrgt_i与机器人当前速度Vcur_i的速度差小于速度同步允差V_ε，满足速度同步允差要求；

若当前各项参数同时满足条件四和条件五，则跳转至步骤5；若有一个条件不满足，则先执行插补运动再进行超时校验；

若超时，则机器人结束本次追踪同步运动并退出至安全位置；若执行插补运动后不超时，则重新进行步骤4；

步骤5、完成追踪进入同步阶段；

执行插补运动直至接收到同步终止命令或超出行程范围时，结束并退出至安全位置。

进一步的，在步骤3的当前插补周期中，采用以下方式进行动态规划：

步骤3.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤3.2、求解当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i＝|Strgt_i-Scur_i|；

步骤3.3、同步阶段跳转条件判断：若当前各项参数同时满足条件一、条件二、条件三，则跳转至步骤4进入减速同步阶段的动态规划，若有一个条件不满足，则进入步骤步骤3.4继续本阶段的动态规划；

步骤3.4、执行插补运动：为使机器人平稳快速启动跟踪，可选用多种速度规划方式，如梯形规划、S型曲线规划、三角函数曲线规划等，本方案以梯形规划进行说明；在当前插补周期中，求解本周期的插补输出位置Scur_i+1与插补输出速度Vcur_i+1，同时记录本阶段实际时间t_a，将其数值递增插补周期dt以备校验，具体的表达式如下：

步骤3.5、超时校验：由于传送带存在速度波动，当波动值过大时，可能一直无法满足同步阶段跳转条件，机器人存在超速风险，因此在执行插补过程中实时记录本阶段的实际时间t_a，该值由影响系数k_a和传送带无速度波动情况下的参考时间T_a共同约束，其中影响系数k_a设置相应的取值范围，可根据传送带速度波动情况灵活设置；求解参考时间T_a时可建立如下关系式：

进一步求解可得

当t_a>k_a·T_a时，则认为加速同步失败，此时不再进行同步阶段跳转，机器人结束本次追踪同步运动并退出至安全位置；

步骤3.6、若本周期中无同步阶段跳转且未因超时校验退出，则在下一个插补周期中重复步骤3.1至3.5进行动态规划。

进一步的，在步骤4的当前插补周期中，采用以下方式进行动态规划：

步骤4.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤4.2、求解当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i，其表达式为ds_i＝|Strgt_i-Scur_i|；

步骤4.3、同步条件判断：若当前各项参数同时满足条件四和条件五，则跳转至步骤5，即机器人与目标物体实现同步；若有一个条件不满足，则进入步骤4.4继续本阶段的动态规划；

步骤4.4、求解期望加速度a_i：定义期望加速度a_i为机器人在当前状态下能够与目标物体实现位置与速度同步的加速度，其表达式为

步骤4.5、执行插补运动：为使机器人尽快逼近目标物体以实现位置与速度的同步，可选用多种速度规划方式，如梯形规划、S型曲线规划、三角函数曲线规划等，本方案以梯形规划为例进行说明；在当前插补周期中，求解本周期的插补输出位置Scur_i+1与插补输出速度Vcur_i+1，同时记录本阶段实际时间t_d，将其数值递增插补周期dt以备校验，具体的表达式如下：

步骤4.6、超时校验：由于传送带存在速度波动，当波动值过大时，可能导致本阶段一直无法满足同步条件，此时机器人存在运动超出行程范围以及撞机风险，因此在执行步骤4.5的插补过程中实时记录本阶段的实际时间t_d，该值由影响系数k_d和传送带无速度波动情况下的参考时间T_d共同约束，其中影响系数k_d设置相应的取值范围，可根据传送带速度波动情况灵活设置；参考时间T_d可通过联立下式求解：

进一步求解可得

当t_d>k_d·T_d时，则认为减速同步失败，此时机器人结束本次追踪同步运动并退出至安全位置；

步骤4.7、若本周期中未进入同步阶段且未因超时校验退出，则在下一个插补周期中重复步骤4.1至4.6进行动态规划。

进一步的，步骤5具体为：

步骤5.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤5.2、执行插补运动：由于此时位置差与速度差均满足同步允差要求，将目标位置Strgt_i与目标速度Vtrgt_i直接作为当前周期的插补输出，具体表达式如下：

步骤5.3、根据实际工艺需求执行轨迹运动：此时机器人已实现与目标物体的同步，可以在同步运动的基础上进一步叠加机器人常规的轨迹运动(如直线、圆弧轨迹)以满足实际工艺需求，若无特殊需求，则本步骤中不叠加运动轨迹，机器人仅与目标物体同步运动；

步骤5.4、若本周期中机器人未接收到同步终止命令且未超出行程范围，则在下一个插补周期中重复步骤5.1至5.3继续执行同步阶段的运动，否则机器人结束本次追踪同步运动并退出至安全位置。

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

一、本发明提出的方法无需提前预测目标物体的位置，而是基于当前插补周期下目标物体的位置与速度动态规划机器人的运动轨迹，运算结果更为准确；

二、本发明提出的方法考虑传送带速度存在波动的情况，能够满足实际现场应用，同时能够保证一定的同步精度；

三、在本发明提出的规划方法中设计了相应的调节系数，可以根据不同的机型及现场设备情况灵活设置以满足应用需求；

四、当机器人末端与目标物体实现同步后，支持进一步根据实际应用中的工艺需求执行复杂的运动轨迹；

五、本发明提出的规划方法具备超时校验，可以保证实际运行过程中设备的安全问题，例如机器人超速、超行程范围、撞机等；

六、整个规划方法运算量小，能够满足实时插补计算的要求，易于实现。

总的来说，本发明考虑传送带速度波动的影响，基于当前插补周期下目标物体的位置与速度动态规划机器人的运动轨迹，整个规划过程运算量小，能够满足实时插补计算的要求，且无需预测目标物体的位置，能够保证一定的同步精度。在追踪过程中具备安全校验，可以保证实际运行过程中设备的安全性。当机器人末端与目标物体实现同步后，可进一步根据实际应用中的工艺需求执行复杂的轨迹运动。

附图说明

图1为本发明的工作流程图；

图2为理想情况下基于速度曲线的运动阶段划分示意图；

图3为实施例中机器人当前速度与目标速度的曲线关系图；

图4为实施例中机器人当前位置与目标位置的曲线关系图；

具体实施方式

为能清楚说明本专利的技术特点，下面通过具体实施方式，并结合其附图1-4，对本专利进行详细阐述。

步骤1、确定本实施例中涉及的各项参数及定义如下：

1.1、设用户设置的传送带参考速度Vcvy＝100mm/s；

1.2、获取当前插补周期中的目标位置为Strgt_i、目标速度为Vtrgt_i、机器人当前位置为Scur_i、当前速度为Vcur_i、插补输出位置为Scur_i+1、插补输出速度为Vcur_i+1、位移差为ds_i；

1.3、设机器人跟随加速度a_m＝3000mm/s²、定义期望加速度为a_i；

1.4、设插补周期dt＝0.004s；

1.5、定义位移同步允差为S_ε、速度同步允差为V_ε；

1.6、定义同步阶段跳转系数为k_s；

1.7、定义加速同步阶段实际时间为t_a、加速同步阶段参考时间为T_a、加速同步阶段影响系数为k_a；定义减速同步阶段实际时间为t_d、减速同步阶段参考时间为T_d、减速同步阶段影响系数为k_d。

步骤2、划分阶段：如图2所示，在理想情况下，机器人从静止状态开始追踪运动中的目标物体并最终与其保持同步，一定会经历一个先加速至超过目标物体参考速度后再减速至目标物体参考速度的过程；进一步地，将整个追踪与同步过程按顺序划分为三个阶段，分别为加速同步阶段、减速同步阶段、同步阶段。

步骤3、进行加速同步阶段的动态规划：本阶段的主要目的是让机器人平稳启动并缩小与目标物体之间的位移差；在本阶段中设置三个同步阶段跳转条件；条件一：Scur_i<Strgt_i，表示机器人的当前位置小于目标位置，即尚未实现同步；条件二：Vcur_i>Vtrgt_i，表示机器人当前速度已经超过目标速度，位移差正在不断减小；条件三：ds_i≤k_s·Scur_i，表示当前插补周期中的位移差已减小至一定范围，可进行同步阶段跳转；其中同步阶段跳转系数k_s的取值范围为(0,1)，其取值决定了加速同步阶段中机器人能达到的最大速度，在保证机器人不超速的情况下可根据实际应用需求灵活设置；在本实施例中，令

在当前插补周期中，采用以下方式进行动态规划：

步骤3.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤3.2、求解当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i＝|Strgt_i-Scur_i|；

步骤3.3、同步阶段跳转条件判断：若当前各项参数同时满足条件一、条件二、条件三，则跳转至步骤4进入减速同步阶段的动态规划，若有一个条件不满足，则进入步骤步骤3.4继续本阶段的动态规划；

步骤3.4、执行插补运动：本实施例采用梯形规划求解本周期的插补输出位置Scur_i+1与插补输出速度Vcur_i+1，同时记录本阶段实际时间t_a，将其数值递增插补周期dt以备校验，具体表达式如下：

步骤3.5、超时校验：在执行步骤3.4的插补过程中实时记录本阶段的实际时间t_a，由影响系数k_a和参考时间T_a共同约束，在本实施例中，令k_a＝4、

随着加速同步阶段的进行，实际时间t_a不断增加，当t_a>2s时，认为加速同步失败，此时不再进行同步阶段跳转，机器人结束本次追踪同步运动并退出至安全位置；

步骤3.6、若本周期中无同步阶段跳转且未因超时校验退出，则在下一个插补周期中重复步骤3.1至3.5进行动态规划。

步骤4、进行减速同步阶段的动态规划：本阶段的主要目的是让机器人的当前位置与速度不断逼近目标物体的位置与速度；本阶段中设置两个同步条件；条件四：ds_i≤S_ε，即当前插补周期的位移差满足位移同步允差要求；条件五：|Vcur_i-Vtrgt_i|≤V_ε，即当前插补周期的速度差满足速度同步允差要求；在本实施例中，令S_ε＝0.5mm、V_ε＝0.1mm/s；在当前插补周期中，采用以下方式进行动态规划：

步骤4.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤4.2、求解当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i＝|Strgt_i-Scur_i|；

步骤4.3、同步条件判断：若当前各项参数同时满足条件四和条件五，则跳转至步骤5，即机器人与目标物体实现同步；若有一个条件不满足，则进入步骤4.4继续本阶段的动态规划；

步骤4.4、求解期望加速度a_i：本实施例采用梯形规律进行求解，即

步骤4.5、执行插补运动；本实施例采用梯形规划求解本周期的插补输出位置Scur_i+1与插补输出速度Vcur_i+1，同时记录本阶段实际时间t_d，将其数值递增插补周期dt以备校验，具体表达式如下：

步骤4.6、超时校验：在执行插补过程中实时记录本阶段的实际时间t_d，由影响系数k_d和参考时间T_d共同约束；本实施例中，取k_d＝50、

随着减速同步阶段的进行，实际时间t_d不断增加，当t_d>2.5s时，则认为减速同步失败，此时机器人结束本次追踪同步运动并退出至安全位置；

步骤4.7、若本周期中未进入同步阶段且未因超时校验退出，则在下一个插补周期中重复步骤4.1至4.6进行动态规划；

步骤5、完成追踪进入同步阶段：

步骤5.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤5.2、执行插补运动：由于此时位置差与速度差均满足同步允差要求，将目标位置Strgt_i与目标速度Vtrgt_i直接作为当前周期的插补输出，其表达式如下：

步骤5.3、在下一个插补周期中重复步骤5.1至5.2继续执行同步阶段的运动，此时整个同步过程中机器人当前速度与目标速度的曲线关系图如图3所示，机器人当前位置与目标位置的曲线关系图如图4所示；若本周中机器人接收到同步终止命令或超出行程范围，则结束本次追踪同步运动并退出至安全位置。

本发明的关键技术点如下：

一、基于动态规划的思想，由当前插补周期中目标物体的位置与速度实时规划机器人的运动轨迹；

二、由追踪过程中机器人当前速度与目标速度的变化关系，基于从静止开始的追踪过程一定存在先加速至超过目标物体参考速度后再减速至目标物体参考速度的思想，将整个追踪与同步过程按顺序划分为三个阶段，分别为加速同步阶段、减速同步阶段、同步阶段；

三、在加速同步阶段中进行动态规划，基于当前插补周期中的目标位置与目标速度，采用梯形规划的方式使机器人平稳快速启动；在该阶段中设置了三个同步阶段跳转条件；条件一：Scur_i<Strgt_i，表示机器人当前位置小于目标位置，即尚未实现同步；条件二：Vcur_i>Vtrgt_i，表示机器人当前速度已经超过目标速度，位移差正在不断减小；条件三：ds_i≤k_s·Scur_i，表示当前插补周期中的位移差已减小至一定范围，可进行同步阶段跳转，其中同步阶段跳转系数k_s可根据实际应用需求灵活设置；在该阶段中只有同时满足三个同步阶段跳转条件，才能转入减速同步阶段；

四、在减速同步阶段中进行动态规划，基于当前插补周期中的目标位置与目标速度，采用梯形规划的方式，动态求解每个插补周期的期望加速度，使机器人的当前位置与速度不断逼近目标物体的位置与速度；在该阶段中设置了两个同步条件；条件四：ds_i≤S_ε，即当前插补周期的位移差满足位移同步允差要求；条件五：|Vcur_i-Vtrgt_i|≤V_ε，即当前插补周期的速度差满足速度同步允差要求，其中允差参数S_ε与V_ε可以根据实际应用需求灵活设置以满足不同的同步精度要求；在该阶段中只有同时满足两个同步条件，才能转入同步阶段；

五、在加速同步阶段与减速同步阶段中均具有超时校验，考虑传送带速度波动带来的不确定性，在求解传送带无速度波动情况下同步阶段参考时间T_a与T_d的基础上，设计影响系数k_a与k_d作用于参考时间，且该系数可以根据实际应用需求灵活调节，通过当前阶段实际插补时间与参考时间的约束关系，保证实际运行过程中设备的安全性；

六、在同步阶段中，支持在同步运动的基础上进一步叠加机器人常规的轨迹运动(如直线、圆弧轨迹)以满足实际工艺需求，若无特殊需求，则不再叠加运动轨迹，机器人仅与目标物体保持同步运动。

本发明具体实施途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (4)

1.一种用于目标同步的机器人动态轨迹规划方法，其特征在于，按以下步骤进行：

步骤1、参数定义及轨迹参数给定；

输入用户设置的传送带参考速度Vcvy；输入机器人跟随加速度a_m以及插补周期dt；

定义期望加速a_i、位移同步允差S_ε、速度同步允差V_ε；定义同步阶段跳转系数k_s；定义加速同步阶段实际时间t_a、加速同步阶段参考时间T_a、加速同步阶段影响系数k_a、减速同步阶段实际时间t_d、减速同步阶段参考时间T_d、减速同步阶段影响系数k_d；

步骤2、划分阶段；

将整个追踪与同步过程按顺序划分为三个阶段，分别为加速同步阶段、减速同步阶段、同步阶段；

步骤3、进行加速同步阶段的动态规划；

设置三个同步阶段跳转条件，条件一：当前插补周期中机器人的当前位置Scur_i小于目标位置Strgt_i；条件二：当前插补周期中机器人当前速度Vcur_i大于目标速度Vtrgt_i；条件三：当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i小于机器人当前位置Scur_i和同步阶段跳转系数k_s的乘积；

若当前各项参数同时满足条件一、条件二、条件三，则跳转至步骤4进入减速同步阶段的动态规划，若有一个条件不满足，则先执行插补运动再进行超时校验；

若超时，则机器人结束本次追踪同步运动并退出至安全位置；若执行插补运动后不超时，则重新进行步骤3；

步骤4、进行减速同步阶段的动态规划；

设置两个同步条件，条件四：当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i小于位移同步允差S_ε；条件五：即当前插补周期中目标速度Vtrgt_i与机器人当前速度Vcur_i的速度差小于速度同步允差V_ε，满足速度同步允差要求；

若当前各项参数同时满足条件四和条件五，则跳转至步骤5；若有一个条件不满足，则先执行插补运动再进行超时校验；

若超时，则机器人结束本次追踪同步运动并退出至安全位置；若执行插补运动后不超时，则重新进行步骤4；

步骤5、完成追踪进入同步阶段；

执行插补运动直至接收到同步终止命令或超出行程范围时，结束并退出至安全位置。

2.根据权利要求1所述的一种用于目标同步的机器人动态轨迹规划方法，其特征在于，在步骤3的当前插补周期中，采用以下方式进行动态规划：

步骤3.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤3.2、求解当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i＝|Strgt_i-Scur_i|；

步骤3.3、同步阶段跳转条件判断：若当前各项参数同时满足条件一、条件二、条件三，则跳转至步骤4进入减速同步阶段的动态规划，若有一个条件不满足，则进入步骤步骤3.4继续本阶段的动态规划；

步骤3.4、执行插补运动：在当前插补周期中，求解本周期的插补输出位置Scur_i+1与插补输出速度Vcur_i+1，同时记录本阶段实际时间t_a，将其数值递增插补周期dt以备校验，具体的表达式如下：

步骤3.5、超时校验：在执行插补过程中实时记录本阶段的实际时间t_a，该值由影响系数k_a和传送带无速度波动情况下的参考时间T_a共同约束，其中影响系数k_a设置相应的取值范围，可根据传送带速度波动情况设置；求解参考时间T_a时可建立如下关系式：

进一步求解可得

当t_a>k_a·T_a时，则认为加速同步失败，此时不再进行同步阶段跳转，机器人结束本次追踪同步运动并退出至安全位置；

步骤3.6、若本周期中无同步阶段跳转且未因超时校验退出，则在下一个插补周期中重复步骤3.1至3.5进行动态规划。

3.根据权利要求1所述的一种用于目标同步的机器人动态轨迹规划方法，其特征在于，在步骤4的当前插补周期中，采用以下方式进行动态规划：

步骤4.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤4.2、求解当前插补周期中机器人当前位置Scur_i与目标位置Strgt_i之间的位移差ds_i，其表达式为ds_i＝|Strgt_i-Scur_i|；

步骤4.3、同步条件判断：若当前各项参数同时满足条件四和条件五，则跳转至步骤5；若有一个条件不满足，则进入步骤4.4继续本阶段的动态规划；

步骤4.4、求解期望加速度a_i：定义期望加速度a_i为机器人在当前状态下能够与目标物体实现位置与速度同步的加速度，其表达式为

步骤4.5、执行插补运动：在当前插补周期中，求解本周期的插补输出位置Scur_i+1与插补输出速度Vcur_i+1，同时记录本阶段实际时间t_d，将其数值递增插补周期dt以备校验，具体的表达式如下：

步骤4.6、超时校验：在执行步骤4.5的插补过程中实时记录本阶段的实际时间t_d，该值由影响系数k_d和传送带无速度波动情况下的参考时间T_d共同约束，其中影响系数k_d设置相应的取值范围，可根据传送带速度波动情况设置；参考时间T_d可通过联立下式求解：

进一步求解可得

当t_d>k_d·T_d时，则认为减速同步失败，此时机器人结束本次追踪同步运动并退出至安全位置；

步骤4.7、若本周期中未进入同步阶段且未因超时校验退出，则在下一个插补周期中重复步骤4.1至4.6进行动态规划。

4.根据权利要求1所述的一种用于目标同步的机器人动态轨迹规划方法，其特征在于，步骤5具体为：

步骤5.1、通过用户配置的传送带编码器单元获取当前插补周期中的目标位置Strgt_i与目标速度Vtrgt_i，通过机器人控制器获取机器人的当前位置Scur_i与当前速度Vcur_i；

步骤5.2、执行插补运动：由于此时位置差与速度差均满足同步允差要求，将目标位置Strgt_i与目标速度Vtrgt_i直接作为当前周期的插补输出，具体表达式如下：

步骤5.3、根据实际工艺需求执行轨迹运动：此时机器人已实现与目标物体的同步，可以在同步运动的基础上进一步叠加机器人常规的轨迹运动以满足实际工艺需求，若无特殊需求，则本步骤中不叠加运动轨迹，机器人仅与目标物体同步运动；

步骤5.4、若本周期中机器人未接收到同步终止命令且未超出行程范围，则在下一个插补周期中重复步骤5.1至5.3继续执行同步阶段的运动，否则机器人结束本次追踪同步运动并退出至安全位置。

Images