[发明专利]一种非实时系统和机器人控制器的双插补方法及装置

权利要求书

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

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

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

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

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

按指定时间序列向控制器发送第三轨迹点的方法为，从非实时系统下发经过补偿处理的第一次插补生成的第三轨迹点，指定控制器接收到的第三轨迹点间隔为固定周期T1；

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

第三轨迹点在控制器中进行第二次插补并执行的方法为，控制器接收到处理过的第三轨迹点后，在控制器中进行第二次插补，插补算法为五次多项式插补算法，在控制器中插补周期为T2，通过第二次插补在下发时间间隔T1之内生成时间间隔为T2的一系列轨迹点。

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

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

步骤2.1，由五次多项式得到从起始点到终止点间的轨迹方程为：θ(t)＝a₀+a₁·t+a₂·t²+a₃·t³+a₄·t⁴+a₅·t⁵，可推出速度关于时间的表达式为：可推出加速度关于时间的表达式为：t表示时间，a₀、a₁、a₂、a₃、a₄、a₅分别表示多项式的系数；

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

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

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

得到从起始点到终止点间的轨迹方程的系数a₀、a₁、a₂、a₃、a₄和a₅的值；

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

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

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

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

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

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

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

按指定时间序列向控制器发送第三轨迹点的方法为，从非实时系统下发经过补偿处理的第一次插补生成的第三轨迹点，指定控制器接收到的第三轨迹点间隔为固定周期T1；

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

第三轨迹点在控制器中进行第二次插补并执行的方法为，控制器接收到处理过的第三轨迹点后，在控制器中进行第二次插补，插补算法为五次多项式插补算法，在控制器中插补周期为T2，通过第二次插补在下发时间间隔T1之内生成时间间隔为T2的一系列轨迹点。