CN109933008A - 一种非实时系统和机器人控制器的双插补方法及装置 - Google Patents

Abstract

本公开提供了一种非实时系统和机器人控制器的双插补实现方法，其特征在于：本公开在非实时系统的轨迹规划工具规划出轨迹后做一次插补，然后在插补后的轨迹点添加补偿值，将补偿后的轨迹点逐个下发到控制器，最后在控制器中做第二次插补的方式，解决使用非实时系统的轨迹规划工具规划出的轨迹同时使用补偿值的问题；通过双插补的方法，解决了非实时系统与实际机器人控制器(实时系统)的点位同步的问题；从非实时系统进行规划和补偿后，向控制器逐个下发轨迹点时，采用提前半个周期的方式下发，保证了轨迹执行的准确性。

Description

一种非实时系统和机器人控制器的双插补方法及装置

技术领域

本公开涉及工业机器人及插补算法技术领域，更具体地说，涉及一种非实时系统和机器人控制器的双插补方法及装置。

背景技术

随着机器人技术的快速发展和人力成本的提高，工业机器人在制造、加工和物流等领域得到了大规模得应用。同时工业机器人控制系统作为一个实时系统，功能不断完善，性能不断增强，已经发展到了较高的水平。

与此同时，也出现了一些非实时系统的机器人相关的工具。这些非实时系统的工具通常处在机器人控制系统之上，用于实现机器人轨迹规划，路径优化等功能。例如ROS系统(开源机器人操作系统，Robot Operating System)中集成的Moveit功能包，它是由运动规划、操作控制、控制与导航算法等功能包组成的集成化开发平台。利用Moveit功能包可对机器人的运动轨迹进行规划，但由于其为非实时系统，规划出的点位无法满足驱动机器人关节运动的要求。又例如进行阻抗控制时，需要根据当前轨迹点的关节角、力矩大小，实时计算出阻抗位移偏移量，并将阻抗位移偏移量作为补偿值叠加到轨迹点，如果直接在机器人控制系统中添加这一功能时，需要结合机器人控制系统进行深度开发，工作量巨大且可能破坏原有机器人控制系统的稳定性。这时考虑在机器人控制系统之上的非实时系统上实现阻抗控制功能，实现该功能需要考虑非实时系统与实时系统同步的问题，同时实现阻抗控制功能还需要考虑点位下发的时机问题。

因此，亟待设计出一种实现方法，解决在非实时系统进行运动规划和在实际机器人控制器中执行的不同步的问题。

发明内容

本公开提供一种非实时系统和机器人控制器的双插补方法及装置，本公开所要解决的技术问题是克服现有在非实时系统中进行运动规划和在实际机器人控制器中执行的不同步的问题，首先由非实时系统之上集成的工具规划出一段轨迹，再将该段轨迹使用五次多项式插补算法(或采用其他插补算法)进行第一次插补，得到一段较平滑的曲线的轨迹点，然后由第三方模块实时计算当前轨迹点的位移偏移量，将该位移偏移量作为补偿值叠加到由第一次插补计算出的轨迹点上，最后将补偿后的轨迹点逐个发送到机器人控制器，在控制器的插补器中集成的算法进行第二次插补，即可解决在实时系统中进行运动规划和在实际机器人控制器(实时系统)中执行导致的不同步问题。

为了达到上述目的，本公开的方法包括以下步骤：

步骤1，规划出机器人运动的起始点到终止点的第一轨迹点；

步骤2，使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点；

步骤3，在第二轨迹点添加补偿值得到第三轨迹点；

步骤4，按指定时间序列向控制器发送第三轨迹点；

步骤5，第三轨迹点在控制器中进行第二次插补并执行。

进一步地，在步骤1中，规划出机器人运动的起始点到终止点的第一轨迹点的方法为，在非实时系统中，使用非实时系统集成的运动规划工具，载入一个机器人模型，将机器人模型末端拖动到另外一个位置和姿态或随机生成一个机器人末端位姿，然后启用规划功能，规划出起始点到终止点轨迹得到第一轨迹点，最后将轨迹点的关节角、速度和加速度信息进行保存和发布。

进一步地，在步骤2中，使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点的方法为，由于需要在插补时使用到补偿值，同时为了使在S1阶段生成的轨迹点能为机器人控制器所用，因此本公开设计出一个粗插补过程，该插补过程作为双插补实现方法的第一次插补过程，考虑到由非实时系统上的规划工具规划出的第一轨迹点包含位移、速度和加速度信息，在选用插补算法时应当选用能将第一轨迹点的位移、速度和加速度作为约束的算法，同时考虑到在机器人控制器的插补器中会进行更精细的第二次插补，因此第一次插补阶段选用具备位移、速度和加速度约束条件，同时计算速度较快的五次多项式插补算法。

五次多项式插补算法实现过程如下：

步骤2.1，由五次多项式的关节角位移关于时间的表达式θ(t)＝a₀+a₁·t+a₂·t²+a₃·t³+a₄·t⁴+a₅·t⁵，可推出速度关于时间的表达式为：可推出加速度关于时间的表达式为：t表示时间，a表示多项式的系数；

步骤2.2，将由非实时系统上的规划工具规划出的第一轨迹点中两相邻的轨迹点的位移、速度和加速度信息代入步骤2.1的表达式中，可得出联立方程组：

其中，t₀表示起始点时刻，为简化计算取t₀＝0，t_f表示终止点时刻；

步骤2.3，计算出五次多项式的系数，即：

求得五次多项式系数即可求出从起始点到终止点间的轨迹方程θ(t)＝a₀+a₁·t+a₂·t²+a₃·t³+a₄·t⁴+a₅·t⁵；

步骤2.4，根据从起始点到终止点间的轨迹方程得到第二轨迹点，即给定固定的时间周期T(例如设T＝100ms，其中ms为时间单位：毫秒)，代入轨迹方程即可求得每个时间周期T对应的关节角，对轨迹方程求一次导数和二次导数可分别求出对应的轨迹点的速度和加速度。

进一步地，在步骤3中，在第二轨迹点添加补偿值得到第三轨迹点的方法为，处理第一次插补生成的第二轨迹点。由步骤2第一次插补生成一系列间隔为指定周期为T的第二轨迹点后，需要在第二轨迹点的每个轨迹点添加补偿值得到第三轨迹点。在阻抗控制案例中，需要根据当前点的关节角和力矩大小，计算出当前点的关节角偏移量，即补偿值，形如下式：delta_θ＝calcOffset(θ,force)，其中θ是当前的关节角，force是当前关节角下的力矩大小，delta_θ是在当前关节角大小和力矩大小下计算出的关节角偏移量，当下发给控制器时需要将关节角θ加上关节角偏移量delta_θ，即在第二轨迹点的每个轨迹点添加补偿值得到第三轨迹点，即下发的关节角为(θ₊delta_θ)。需要说明的是，对轨迹点的处理并不局限于关节角，需要对关节速度和加速度处理时，上述方法同样适用。

进一步地，在步骤4中，按指定时间序列向控制器发送第三轨迹点的方法为，从非实时系统下发经过补偿处理的第一次插补生成的第三轨迹点，与现有的方法的区别在于，传统的一次性将全部的轨迹点下发给控制器的方式，本公开采用逐个轨迹点下发的方式。指定控制器接收到的第三轨迹点间隔为固定周期T1(例如设T1＝100ms，其中ms为时间单位：毫秒)，则实际控制器接收轨迹点的时间序列为0,100ms,200ms,300ms……。考虑到收发数据有一定的时间延迟，发送数据的时间序列相对于接收的时间序列应相对提前，本公开采用0,50ms,150ms,250ms……的时间序列向控制器发送第三轨迹点，即在第50ms时向控制器发送规划的第100ms将执行的处理后的第一次插补轨迹点，在第150ms时向控制器发送规划的第200ms将执行的处理后的第一次插补轨迹点，以此类推，发送的时间序列比按规划执行的时间序列提前了50ms,保证了轨迹的准确性。需要说明的是，指定的固定周期应当根据实际情况合理取用，通常情况下固定周期T越小，轨迹的平滑度越高，同时运算量也越大。

进一步地，在步骤5中，第三轨迹点在控制器中进行第二次插补并执行的方法为，第三轨迹点在控制器中进行第二次插补并执行，在前面的步骤中，已经完成了非实时系统的规划工具进行的轨迹规划，第一次插补，为轨迹点添加补偿值，下发给控制器等工作，控制器接收到处理过的第三轨迹点后，需要在控制器中进行第二次插补，插补算法仍为五次多项式插补算法或其他插补算法，插补算法位于控制器中，用于保证机器人运动轨迹更光顺的，插补得到更精细的轨迹点，以驱动机器人关节运动。在控制器中插补周期比第一次插补的周期更小，设为T2(例如设T＝4ms，其中ms为时间单位：毫秒)，通过本次插补在下发时间间隔T1之内生成时间间隔为T2的一系列轨迹点。

本公开还提供了一种非实时系统和机器人控制器的双插补装置，所述装置包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序运行在以下装置的单元中：

第一轨迹点规划单元，用于规划出机器人运动的起始点到终止点的第一轨迹点；

第一次插补单元，用于使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点；

轨迹补偿单元，用于在第二轨迹点添加补偿值得到第三轨迹点；

定时发送单元，用于按指定时间序列向控制器发送第三轨迹点；

第二次插补单元，用于第三轨迹点在控制器中进行第二次插补并执行。

本公开的有益效果为以下方面：

1、传统的方式基于非实时系统的规划工具规划出机器人起始点到终止点的轨迹，一次性下发给控制器使用的方式，无法实时使用计算出的补偿值。本公开在非实时系统的规划工具规划出轨迹后做一次五次多项式插补，然后在插补后的轨迹点添加补偿值，将补偿后的轨迹点逐个下发到控制器，最后在控制器中做第二次插补的方式，解决使用非实时系统上的规划工具规划出的轨迹同时使用补偿值的问题。

2、通过双插补的方法，解决了非实时系统与实际机器人控制器(实时系统)的点位同步的问题。

3、从非实时系统进行规划和补偿后，向控制器逐个下发轨迹点时，采用提前半个周期的方式下发，保证了轨迹执行的精度。

附图说明

通过对结合附图所示出的实施方式进行详细说明，本公开的上述以及其他特征将更加明显，本公开附图中相同的参考标号表示相同或相似的元素，显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，在附图中：

图1所示为一种非实时系统和机器人控制器的双插补实现方法的流程图；

图2所示为一种非实时系统和机器人控制器的双插补装置结构图。

具体实施方式

以下将结合实施例和附图对本公开的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本公开的目的、方案和效果。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本公开提出一种非实时系统和机器人控制器的双插补方法,，具体包括以下步骤：

步骤1，规划出机器人运动的起始点到终止点的第一轨迹点；

步骤2，使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点；

步骤3，在第二轨迹点添加补偿值得到第三轨迹点；

步骤4，按指定时间序列向控制器发送第三轨迹点；

步骤5，第三轨迹点在控制器中进行第二次插补并执行。

进一步地，在步骤1中，规划出机器人运动的起始点到终止点的第一轨迹点的方法为，在非实时系统中，使用非实时系统集成的运动规划工具，载入一个机器人模型，将机器人模型末端拖动到另外一个位置和姿态或随机生成一个机器人末端位姿，然后启用规划功能，规划出起始点到终止点轨迹得到第一轨迹点，最后将轨迹点的关节角、速度和加速度信息进行保存和发布。

进一步地，在步骤2中，使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点的方法为，由于需要在插补时使用到补偿值，同时为了使在S1阶段生成的轨迹点能为机器人控制器所用，因此本公开设计出一个粗插补过程，该插补过程作为双插补实现方法的第一次插补过程，考虑到由非实时系统上的规划工具规划出的第一轨迹点包含位移、速度和加速度信息，在选用插补算法时应当选用能将第一轨迹点的位移、速度和加速度作为约束的算法，同时考虑到在机器人控制器的插补器中会进行更精细的第二次插补，因此第一次插补阶段选用具备位移、速度和加速度约束条件，同时计算速度较快的五次多项式插补算法。

五次多项式插补算法实现过程如下：

步骤2.1，由五次多项式的关节角位移关于时间的表达式θ(t)＝a₀+a₁·t+a₂·t²+a₃·t³+a₄·t⁴+a₅·t⁵，可推出速度关于时间的表达式为：可推出加速度关于时间的表达式为：t表示时间，a表示多项式的系数；

步骤2.2，将由非实时系统上的规划工具规划出的第一轨迹点中两相邻的轨迹点的位移、速度和加速度信息代入步骤2.1的表达式中，可得出联立方程组：

其中，t₀表示起始点时刻，为简化计算取t₀＝0，t_f表示终止点时刻；

步骤2.3，计算出五次多项式的系数，即：

求得五次多项式系数即可求出从起始点到终止点间的轨迹方程

θ(t)＝a₀+a₁·t+a₂·t²+a₃·t³+a₄·t⁴+a₅·t⁵；

步骤2.4，根据从起始点到终止点间的轨迹方程得到第二轨迹点，即给定固定的时间周期T(例如设T＝100ms，其中ms为时间单位：毫秒)，代入轨迹方程即可求得每个时间周期T对应的关节角，对轨迹方程求一次导数和二次导数可分别求出对应的轨迹点的速度和加速度。

进一步地，在步骤3中，在第二轨迹点添加补偿值得到第三轨迹点的方法为，处理第一次插补生成的第二轨迹点。由步骤2第一次插补生成一系列间隔为指定周期为T的第二轨迹点后，需要在第二轨迹点的每个轨迹点添加补偿值得到第三轨迹点。在阻抗控制案例中，需要根据当前点的关节角和力矩大小，计算出当前点的关节角偏移量，即补偿值，形如下式：delta_θ＝calcOffset(θ,force)，其中θ是当前的关节角，force是当前关节角下的力矩大小，delta_θ是在当前关节角大小和力矩大小下计算出的关节角偏移量，当下发给控制器时需要将关节角θ加上关节角偏移量delta_θ，即在第二轨迹点的每个轨迹点添加补偿值得到第三轨迹点，即下发的关节角为(θ+delta_θ)。需要说明的是，对轨迹点的处理并不局限于关节角，需要对关节速度和加速度处理时，上述方法同样适用。

进一步地，在步骤4中，按指定时间序列向控制器发送第三轨迹点的方法为，从非实时系统下发经过补偿处理的第一次插补生成的第三轨迹点，与现有的方法的区别在于，传统的一次性将全部的轨迹点下发给控制器的方式，本公开采用逐个轨迹点下发的方式。指定控制器接收到的第三轨迹点间隔为固定周期T1(例如设T1＝100ms，其中ms为时间单位：毫秒)，则实际控制器接收轨迹点的时间序列为0,100ms,200ms,300ms……。考虑到收发数据有一定的时间延迟，发送数据的时间序列相对于接收的时间序列应相对提前，本公开采用0,50ms,150ms,250ms……的时间序列向控制器发送第三轨迹点，即在第50ms时向控制器发送规划的第100ms将执行的处理后的第一次插补轨迹点，在第150ms时向控制器发送规划的第200ms将执行的处理后的第一次插补轨迹点，以此类推，发送的时间序列比按规划执行的时间序列提前了50ms,保证了轨迹的准确性。需要说明的是，指定的固定周期应当根据实际情况合理取用，通常情况下固定周期T越小，轨迹的平滑度越高，同时运算量也越大。

进一步地，在步骤5中，第三轨迹点在控制器中进行第二次插补并执行的方法为，在前面的步骤中，已经完成了非实时系统的规划工具进行的轨迹规划，第一次插补，为轨迹点添加补偿值，下发给控制器等工作，控制器接收到处理过的第三轨迹点后，需要在控制器中进行第二次插补，插补算法仍为五次多项式插补算法或其他插补算法，插补得到更精细的轨迹点，以驱动机器人关节运动。在控制器中插补周期比第一次插补的周期更小，设为T2(例如设T2＝4ms，其中ms为时间单位：毫秒)，通过本次插补在下发时间间隔T1之内生成时间间隔为T2的一系列轨迹点。

优选地，本公开的一种实施案例的应用场景中，工控机中预装有ROS系统，本实施案例使用的ROS系统版本为indigo。本实施案例使用的机器人为某机器人有限公司自主研发的六关节机器人，该机器人的控制器与工控机通过网线相连；

如图1所示是本实施案例提供的一种非实时系统和机器人控制器的双插补实现方法的流程图，具体的实施方式，包括如下步骤：

S1，搭建实施案例平台。将预装有indigo版本的ROS系统(非实时系统)的工控机与六关节机器人控制柜使用网线连接；启动ROS系统中的Rviz工具，配置好六关节机器人的Moveit文件和参数，在Rviz工具中导入六关节机器人的三维模型。将在Rviz中六关节2机器人模型末端拖动到另外一个位置和姿态或随机生成一个机器人末端位姿，然后在它的Moveit模块点击Plan按钮启用规划功能，规划出起始点到终止点轨迹，然后通过Action通信机制将轨迹点的关节角、速度和加速度信息进行发布。

S2，对Moveit规划出的轨迹点进行第一次插补处理。如发明内容中的描述，采用五次多项式插补算法对轨迹点进行插值处理。通过监听Action消息获取由Moveit规划出的轨迹序列(P1,P2,P3,...Pk...Pn)，其中Pk表示轨迹上的轨迹点位，其中包含机器人的6个关节角、速度和加速度信息。给定固定的插补周期T＝100ms，按照发明内容中的所述的处理方法，将该段轨迹进行五次多项式插补算法处理后得到新的轨迹(P11,P12,P13,P21,P22,P23...Pk1,Pk1,Pk2,Pk3...)

S3，处理第一次插补生成的轨迹点，将计算出的补偿值叠加到第一次插补生成的插补点。本实施案例应用于阻抗控制中，只需要补偿关节角。本实施案例通过输入目标轨迹点下的关节角和力矩大小计算出在当前点的6个关节角补偿值，将补偿值叠加到上一个步骤中生成的轨迹序列后，得到新的轨迹序列(P'11,P'12,P'13,P'21,P'22,P'23...P'k1,P'k1,P'k2,P'k3...)

S4，从ROS系统下发经过补偿处理的第一次插补生成的轨迹点。这一步骤将S3步骤中生成的轨迹序列点通过TCP/IP方式下发给六关节机器人控制器。由S2步骤知，轨迹点的插补周期为T＝100ms,为了保证控制器收到的目标轨迹点不出现延时，第2个轨迹点从50ms就开始发送，第3个点从150ms开始发送，第4个点从250ms开始发送，以此类推保证提前50ms将目标轨迹点位下发。

S5，第三轨迹点在控制器中进行第二次插补并执行。将S4步骤下发的点位导入到机器人控制器中的插补器中进行第二次插补计算，第二次插补计算采用的插补算法位于控制器中，用于保证机器人运动轨迹更光顺的，插补得到更精细的轨迹点，以驱动机器人关节运动。

通过以上步骤即可实现本公开所述的基于ROS和机器人控制器的双插补实现方法。

本公开的实施例提供的一种非实时系统和机器人控制器的双插补装置，如图2所示为本公开的一种非实时系统和机器人控制器的双插补装置图，该实施例的一种非实时系统和机器人控制器的双插补装置包括：处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述一种非实时系统和机器人控制器的双插补装置实施例中的步骤。

所述装置包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序运行在以下装置的单元中：

第一轨迹点规划单元，用于规划出机器人运动的起始点到终止点的第一轨迹点；

第一次插补单元，用于使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点；

轨迹补偿单元，用于在第二轨迹点添加补偿值得到第三轨迹点；

定时发送单元，用于按指定时间序列向控制器发送第三轨迹点；

第二次插补单元，用于第三轨迹点在控制器中进行第二次插补并执行。

所述一种非实时系统和机器人控制器的双插补装置可以运行于桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备中。所述一种非实时系统和机器人控制器的双插补装置，可运行的装置可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，所述例子仅仅是一种非实时系统和机器人控制器的双插补装置的示例，并不构成对一种非实时系统和机器人控制器的双插补装置的限定，可以包括比例子更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述一种非实时系统和机器人控制器的双插补装置还可以包括输入输出设备、网络接入设备、总线等。

所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述一种非实时系统和机器人控制器的双插补装置运行装置的控制中心，利用各种接口和线路连接整个一种非实时系统和机器人控制器的双插补装置可运行装置的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述一种非实时系统和机器人控制器的双插补装置的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

尽管本公开的描述已经相当详尽且特别对几个所述实施例进行了描述，但其并非旨在局限于任何这些细节或实施例或任何特殊实施例，而是应当将其视作是通过参考所附权利要求考虑到现有技术为这些权利要求提供广义的可能性解释，从而有效地涵盖本公开的预定范围。此外，上文以发明人可预见的实施例对本公开进行描述，其目的是为了提供有用的描述，而那些目前尚未预见的对本公开的非实质性改动仍可代表本公开的等效改动。

Claims (7)

1.一种非实时系统和机器人控制器的双插补方法，其特征在于，所述方法包括以下步骤：

步骤1，规划出机器人运动的起始点到终止点的第一轨迹点；

步骤2，使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点；

步骤3，在第二轨迹点添加补偿值得到第三轨迹点；

步骤4，按指定时间序列向控制器发送第三轨迹点；

步骤5，第三轨迹点在控制器中进行第二次插补并执行。

2.根据权利要求1所述的一种非实时系统和机器人控制器的双插补方法，其特征在于，在步骤1中，规划出机器人运动的起始点到终止点的第一轨迹点的方法为，在非实时系统中，使用非实时系统集成的运动规划工具，载入一个机器人模型，将机器人模型末端拖动到另外一个位置和姿态或随机生成一个机器人末端位姿，然后启用规划功能，规划出起始点到终止点轨迹得到第一轨迹点，最后将轨迹点的关节角、速度和加速度信息进行保存和发布。

3.根据权利要求1所述的一种非实时系统和机器人控制器的双插补方法，其特征在于，在步骤2中，使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点的方法为，

步骤2.1，由五次多项式的关节角位移关于时间的表达式θ(t)＝a₀+a₁·t+a₂·t²+a₃·t³+a₄·t⁴+a₅·t⁵，可推出速度关于时间的表达式为：可推出加速度关于时间的表达式为：t表示时间，a表示多项式的系数；

步骤2.2，将由非实时系统上的规划工具规划出的第一轨迹点中两相邻的轨迹点的位移、速度和加速度信息代入步骤2.1的表达式中，可得出联立方程组：

其中，t₀表示起始点时刻，为简化计算取t₀＝0，t_f表示终止点时刻；

步骤2.3，计算出五次多项式的系数，即：

求得五次多项式系数即可求出从起始点到终止点间的轨迹方程θ(t)＝a₀+a₁·t+a₂·t²+a₃·t³+a₄·t⁴+a₅·t⁵；

步骤2.4，根据从起始点到终止点间的轨迹方程得到第二轨迹点，即给定固定的时间周期T，代入轨迹方程即可求得每个时间周期T对应的关节角，对轨迹方程求一次导数和二次导数可分别求出对应的轨迹点的速度和加速度。

4.根据权利要求1所述的一种非实时系统和机器人控制器的双插补方法，其特征在于，在步骤3中，在第二轨迹点添加补偿值得到第三轨迹点的方法为，处理第一次插补生成的第二轨迹点，在第二轨迹点的每个轨迹点添加补偿值得到第三轨迹点，根据当前点的关节角和力矩大小，计算出当前点的关节角偏移量，即补偿值，形如下式：delta_θ＝calcOffset(θ,force)，其中θ是当前的关节角，force是当前关节角下的力矩大小，delta_θ是在当前关节角大小和力矩大小下计算出的关节角偏移量，当下发给控制器时需要将关节角θ加上关节角偏移量delta_θ，即在第二轨迹点的每个轨迹点添加补偿值得到第三轨迹点，即下发的关节角为(θ₊delta_θ)。

5.根据权利要求1所述的一种非实时系统和机器人控制器的双插补方法，其特征在于，在步骤4中，按指定时间序列向控制器发送第三轨迹点的方法为，从非实时系统下发经过补偿处理的第一次插补生成的第三轨迹点，指定控制器接收到的第三轨迹点间隔为固定周期T1。

6.根据权利要求5所述的一种非实时系统和机器人控制器的双插补方法，其特征在于，在步骤5中，第三轨迹点在控制器中进行第二次插补并执行的方法为，控制器接收到处理过的第三轨迹点后，在控制器中进行第二次插补，插补算法为五次多项式插补算法，在控制器中插补周期为T2，通过第二次插补在下发时间间隔T1之内生成时间间隔为T2的一系列轨迹点。

7.一种非实时系统和机器人控制器的双插补装置，其特征在于，所述装置包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序运行在以下装置的单元中：

第一轨迹点规划单元，用于规划出机器人运动的起始点到终止点的第一轨迹点；

第一次插补单元，用于使用插补算法对第一轨迹点进行第一次插补得到第二轨迹点；

轨迹补偿单元，用于在第二轨迹点添加补偿值得到第三轨迹点；

定时发送单元，用于按指定时间序列向控制器发送第三轨迹点；

第二次插补单元，用于第三轨迹点在控制器中进行第二次插补并执行。