[发明专利]一种两轴联动机械臂运动控制的同步误差补偿方法

摘要

一种两轴联动机械臂运动控制的同步误差补偿方法，包括以下步骤：1)采用S型加减速算法并结合电机的性能，建立基于频率的速度表、基于脉冲数的步长表和基于步长的加速度表，再进行速度规划得到起始速度、终止速度和匀速速度，进而求出加速长度、减速长度和匀速长度；2)在运动过程中进行同步插补，计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数；3)使用伺服器编码器进行同步误差反馈补偿；4)在运动过程中，将参考轴X轴每次插补周期发送的脉冲数进行累加判断是否运动到终点。本发明提供了一种两轴联动机械臂运动控制的同步误差补偿方法，使得机械臂两轴联动的同步性更好、精度更高。

主权项

1.一种两轴联动机械臂运动控制的同步误差补偿方法，其特征在于：所述方法包括以下步骤：1)采用S型加减速算法并结合电机的性能，建立基于频率的速度表(v0,v1,v2,...,vn)，基于脉冲数的步长表(s0,s1,s2,...,sn)和基于步长的加速度表(a0,a1,a2,...,an)，三者间的关系有ai＝(vi+1‑vi)/si；设在坐标系中有一线段l，其长度为L，与X轴夹角为θ，线段l在X轴上的映射长度大于在Y轴上的映射长度，选取X轴为参考轴，结合速度规划得到起始速度vstart、终止速度vend和匀速速度vcons，则由起始速度vstart得X轴起始速度vX0＝vstart*cosθ，Y轴起始速度vY0＝vstart*sinθ，同理可得，X轴终止速度vXl，Y轴终止速度vYl，X轴匀速速度vXn，Y轴匀速速度vYn，计算X轴加速长度为LXacce、匀速长度为LXcons和减速长度为LXdece，则L＝LXacce+LXcons+LXdece；2)在运动过程中进行同步插补，计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数；计算X轴在第i+1个插补周期的速度vXi+1：vXi+1＝vXi+aj*MXi               (1)其中，vXi为X轴第i个插补周期的速度，加速度aj的下标j为vXi在速度表中(v0,v1,v2,...,vn)的位置，通过搜索vj≤vXi＜vj+1来确定下标j,MXi为X轴第i个插补周期需要发送的脉冲数；同理可得，Y轴第i+1个插补周期的速度vYi+1：vYi+1＝vYi+aj*MYi                (2)所述公式(1)和公式(2)中MXi、MYi的确定方法如下：计算X轴第i个插补周期需要发送的脉冲数MXi：MXi＝int[T*vXi]                   (3)其中，int[]表示对计算结果进行取整，确保得到的脉冲数为整数；T为固定的插补时间，由系统的插补精度要求和硬件的相关参数共同决定；vXi为X轴第i个插补周期的速度，则X轴第i个插补周期TXi为：TXi＝MXi/vXi                   (4)再计算Y轴第i个插补周期需要发送的脉冲数MYi：MYi＝int[TYi*vYi]                   (5)其中，TYi为Y轴第i个插补周期，TYi＝TXi+ΔTYi‑1，ΔTYi‑1为Y轴上一次插补周期计算MYi‑1取整时舍去的小数所对应的时间，vYi为Y轴第i个插补周期的速度，计算TYi*vYi取整时舍去的小数所对应的时间ΔTYi，加到下一次Y轴的插补周期TYi+1中；3)使用伺服器的编码器进行同步误差反馈补偿，根据伺服器编码器实时反馈的位置来确定X轴、Y轴的同步脉冲误差ΔMXi、ΔMYi和同步时间误差ΔtXi、ΔtYi；在X轴第i个插补周期结束时，采集编码器反馈回来的当前插补周期内X轴运行的脉冲数M′Xi，计算X轴在第i个插补周期内的同步脉冲误差ΔMXi和同步时间误差ΔtXi：ΔMXi＝MXi‑M′Xi                 (6)ΔtXi＝ΔMXi/vXi                   (7)同理可得，Y轴在第i个插补周期内的同步脉冲误差M′Yi和同步时间误差ΔtYi：ΔMYi＝MYi‑M′Yi                  (8)ΔtYi＝ΔMYi/vYi                  (9)若ΔtXi＞ΔtYi，说明X轴滞后Y轴，则Y轴第i+1个插补周期更新为T′Yi+1，即T′Yi+1＝TYi+1+(ΔtXi‑ΔtYi),X轴第i+1个插补周期保持不变，即T′Xi+1＝TXi+1；若ΔtXi＜ΔtYi，说明Y轴滞后X轴，则X轴第i+1个插补周期更新为T′Xi+1，即T′Xi+1＝TXi+1+(ΔtYi‑ΔtXi)，Y轴第i+1个插补周期保持不变，即T′Yi+1＝TYi+1。