CN106227154A - 一种两轴联动机械臂运动控制的同步误差补偿方法 - Google Patents

Abstract

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

Description

一种两轴联动机械臂运动控制的同步误差补偿方法

技术领域

本发明属于运动控制技术领域，尤其是一种两轴联动机械臂运动控制的同步误差补偿方法。

背景技术

目前，随着科技的迅速发展，机械臂在各种制造行业广泛应用，使得工业自动化程度不断提高。虽然各种机械臂的形态、结构和功能各不一样，但是它们都有一个共同点，就是要沿着某条轨迹精准的运动到一点。在市场上有很多低端的机械臂控制器与伺服器之间是开环控制，没有对伺服器反馈的脉冲加以利用，若控制器与伺服器之间采取闭环控制，就可以对机械臂运动过程中产生的同步误差进行实时补偿，使得机械臂两轴联动的同步性更好。

发明内容

为了克服现有的两轴联动低端控制器在运动过程中定位精度低、两轴同步性差这些方面的不足，本发明提出了一种两轴联动机械臂运动控制的同步误差补偿方法，主要解决计算时取整误差以及硬件延时和机械响应延时误差，实现了机械臂两轴联动具有精度高、同步性好的特点。

本发明解决其技术问题所采用的技术方案如下：

一种两轴联动机械臂运动控制的同步误差补偿方法，所述方法包括以下步骤：

1)采用S型加减速算法并结合电机的性能，建立基于频率的速度表(v₀,v₁,v₂,...,v_n)，基于脉冲数的步长表(s₀,s₁,s₂,...,s_n)和基于步长的加速度表(a₀,a₁,a₂,...,a_n)，三者间的关系有a_i＝(v_i+1-v_i)/s_i；

设在坐标系中有一线段l，其长度为L，与X轴夹角为θ，线段l在X轴上的映射长度大于在Y轴上的映射长度，选取X轴为参考轴，结合速度规划得到起始速度v_start、终止速度v_end和匀速速度v_cons，则由起始速度v_start得X轴起始速度v_X0＝v_start*cosθ，Y轴起始速度v_Y0＝v_start*sinθ，同理可得，X轴终止速度v_Xl，Y轴终止速度v_Yl，X轴匀速速度v_Xn，Y轴匀速速度v_Yn，计算X轴加速长度为L_Xacce、匀速长度为L_Xcons和减速长度为L_Xdece，则L＝L_Xacce+L_Xcons+L_Xdece；

2)在运动过程中进行同步插补，计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数；

计算X轴在第i+1个插补周期的速度v_Xi+1：

v_Xi+1＝v_Xi+a_j*M_Xi (1)

其中，v_Xi为X轴第i个插补周期的速度，加速度a_j的下标j为v_Xi在速度表中(v₀,v₁,v₂,...,v_n)的位置，通过搜索v_j≤v_Xi＜v_j+1来确定下标j,M_Xi为X轴第i个插补周期需要发送的脉冲数；

同理可得，Y轴第i+1个插补周期的速度v_Yi+1：

v_Yi+1＝v_Yi+a_j*M_Yi (2)

所述公式(1)和公式(2)中M_Xi、M_Yi的确定方法如下：

计算X轴第i个插补周期需要发送的脉冲数M_Xi：

M_Xi＝int[T*v_Xi] (3)

其中，int[]表示对计算结果进行取整，确保得到的脉冲数为整数；T为固定的插补时间，由系统的插补精度要求和硬件的相关参数共同决定；v_Xi为X轴第i个插补周期的速度，则X轴第i个插补周期T_Xi为：

T_Xi＝M_Xi/v_Xi (4)

再计算Y轴第i个插补周期需要发送的脉冲数M_Yi：

M_Yi＝int[T_Yi*v_Yi] (5)

其中，T_Yi为Y轴第i个插补周期，T_Yi＝T_Xi+ΔT_Yi-1，ΔT_Yi-1为Y轴上一次插补周期计算M_Yi-1取整时舍去的小数所对应的时间，v_Yi为Y轴第i个插补周期的速度，计算T_Yi*v_Yi取整时舍去的小数所对应的时间ΔT_Yi，加到下一次Y轴的插补周期T_Yi+1中；

3)使用伺服器的编码器进行同步误差反馈补偿，根据伺服器编码器实时反馈的位置来确定X轴、Y轴的同步脉冲误差ΔM_Xi、ΔM_Yi和同步时间误差Δt_Xi、Δt_Yi。

进一步，在X轴第i个插补周期结束时，采集编码器反馈回来的当前插补周期内X轴运行的脉冲数M'_Xi，计算X轴在第i个插补周期内的同步脉冲误差ΔM_Xi和同步时间误差Δt_Xi：

ΔM_Xi＝M_Xi-M'_Xi (6)

Δt_Xi＝ΔM_Xi/v_Xi (7)

同理可得，Y轴在第i个插补周期内的同步脉冲误差M'_Yi和同步时间误差Δt_Yi：

ΔM_Yi＝M_Yi-M'_Yi (8)

Δt_Yi＝ΔM_Yi/v_Yi (9)

若Δt_Xi＞Δt_Yi，说明X轴滞后Y轴，则Y轴第i+1个插补周期更新为T'_Yi+1，即T′_Yi+1＝T_Yi+1+(Δt_Xi-Δt_Yi),X轴第i+1个插补周期保持不变，即T′_Xi+1＝T_Xi+1；若Δt_Xi＜Δt_Yi，说明Y轴滞后X轴，则X轴第i+1个插补周期更新为T'_Xi+1，即T'_Xi+1＝T_Xi+1+(Δt_Yi-Δt_Xi)，Y轴第i+1个插补周期保持不变，即T′_Yi+1＝T_Yi+1。

更进一步，计算X轴、Y轴每个插补周期的脉冲发送频率，根据所述步骤2)中X轴第i个插补周期需要发送的脉冲数为M_Xi和所述步骤3)中X轴第i个插补周期更新为T′_Xi，得X轴在第i个插补周期的脉冲发送频率为f_Xi：

f_Xi＝M_Xi/T′_Xi (10)

同理可得，Y轴在第i个插补周期的脉冲发送频率为f_Yi：

f_Yi＝M_Yi/T′_Yi (11)

所述同步误差补偿方法还包括以下步骤：

4)在运动过程中，将参考轴X轴每次插补周期发送的脉冲数进行累加判断是否运动到终点；

将X轴每次插补周期发送的脉冲数进行累加得到M_Xadd：

M_Xadd＝M_X0+M_X1+M_X2+...+M_Xi (12)

所述步骤1)中X轴加速长度为L_Xacce、匀速长度为L_Xcons、减速长度为L_Xdece，在加速过程中，所述公式(1)和所述公式(2)中的a_j大于0，当M_Xadd≥L_Xacce时，说明加速过程完成进入匀速过程；在匀速过程中，所述公式(1)和所述公式(2)中的a_j恒为0，当M_Xadd≥L_Xacce+L_Xcons时，提前进入减速过程以确保减速到终止速度v_Xl；在减速过程中，所述公式(1)和所述公式(2)中的a_j小于0，当M_Xadd≥L时，以终止速度v_Xl运行M_Xi-(M_Xadd-L)个脉冲达到线段的终点。

本发明的有益效果为：(1)每一次插补都对上一次插补计算时的误差进行补偿，提高了机械臂两轴联动的精度和同步性；(2)使用同步误差反馈补偿法，更进一步提高了两轴联动的同步性，减小了硬件延时和机械响应延时的误差。

附图说明

图1为同步误差补偿方法的流程图。

具体实施方式

下面结合附图对本发明做进一步说明。

参照图1，一种两轴联动机械臂运动控制的同步误差补偿方法，包括以下步骤：

1)采用S型加减速算法并结合电机的性能，建立基于频率的速度表(v₀,v₁,v₂,...,v_n)，基于脉冲数的步长表(s₀,s₁,s₂,...,s_n)和基于步长的加速度表(a₀,a₁,a₂,...,a_n)，三者间的关系有a_i＝(v_i+1-v_i)/s_i。

设在坐标系中有一线段l，其长度为L，与X轴夹角为θ，线段l在X轴上的映射长度大于在Y轴上的映射长度，选取X轴为参考轴，结合速度规划得到起始速度v_start、终止速度v_end和匀速速度v_cons，则由起始速度v_start得X轴起始速度v_X0＝v_start*cosθ，Y轴起始速度v_Y0＝v_start*sinθ，同理可得，X轴终止速度v_Xl，Y轴终止速度v_Yl，X轴匀速速度v_Xn，Y轴匀速速度v_Yn。计算X轴加速长度为L_Xacce、匀速长度为L_Xcons和减速长度为L_Xdece，则L＝L_Xacce+L_Xcons+L_Xdece。

加速长度

减速长度

匀速长度L_Xcons＝L-L_Xacce-L_Xdece

2)如图1，在运动过程中进行同步插补，计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数。

计算X轴在第i+1个插补周期的速度v_Xi+1：

v_Xi+1＝v_Xi+a_j*M_Xi (1)

其中，v_Xi为X轴第i个插补周期的速度，加速度a_j的下标j为v_Xi在速度表中(v₀,v₁,v₂,...,v_n)的位置，通过搜索v_j≤v_Xi＜v_j+1来确定下标j,M_Xi为X轴第i个插补周期需要发送的脉冲数。

同理可得，Y轴第i+1个插补周期的速度v_Yi+1：

v_Yi+1＝v_Yi+a_j*M_Yi (2)

所述公式(1)和公式(2)中M_Xi、M_Yi的确定方法如下：

计算X轴第i个插补周期需要发送的脉冲数M_Xi：

M_Xi＝int[T*v_Xi] (3)

其中，int[]表示对计算结果进行取整，确保得到的脉冲数为整数；T为固定的插补时间，由系统的插补精度要求和硬件的相关参数共同决定；v_Xi为X轴第i个插补周期的速度。则X轴第i个插补周期T_Xi为：

T_Xi＝M_Xi/v_Xi (4)

再计算Y轴第i个插补周期需要发送的脉冲数M_Yi：

M_Yi＝int[T_Yi*v_Yi] (5)

其中，T_Yi为Y轴第i个插补周期，T_Yi＝T_Xi+ΔT_Yi-1，ΔT_Yi-1为Y轴上一次插补周期计算M_Yi-1取整时舍去的小数所对应的时间，v_Yi为Y轴第i个插补周期的速度。计算T_Yi*v_Yi取整时舍去的小数所对应的时间ΔT_Yi，加到下一次Y轴的插补周期T_Yi+1中。

3)使用伺服器的编码器进行同步误差反馈补偿，根据伺服器编码器实时反馈的位置来确定X轴、Y轴的同步脉冲误差ΔM_Xi、ΔM_Yi和同步时间误差Δt_Xi、Δt_Yi。

进一步，在X轴第i个插补周期结束时，采集编码器反馈回来的当前插补周期内X轴运行的脉冲数M'_Xi，计算X轴在第i个插补周期内的同步脉冲误差ΔM_Xi和同步时间误差Δt_Xi：

ΔM_Xi＝M_Xi-M'_Xi (6)

Δt_Xi＝ΔM_Xi/v_xi (7)

同理可得，Y轴在第i个插补周期内的同步脉冲误差M'_Yi和同步时间误差Δt_Yi：

ΔM_Yi＝M_Yi-M'_Yi (8)

Δt_Yi＝ΔM_Yi/v_Yi (9)

若Δt_Xi＞Δt_Yi，说明X轴滞后Y轴，则Y轴第i+1个插补周期更新为T'_Yi+1，即T′_Yi+1＝T_Yi+1+(Δt_Xi-Δt_Yi),X轴第i+1个插补周期保持不变，即T′_Xi+1＝T_Xi+1；若Δt_Xi＜Δt_Yi，说明Y轴滞后X轴，则X轴第i+1个插补周期更新为T'_Xi+1，即T'_Xi+1＝T_Xi+1+(Δt_Yi-Δt_Xi)，Y轴第i+1个插补周期插保持不变，即T′_Yi+1＝T_Yi+1。

更进一步，计算X轴、Y轴每个插补周期的脉冲发送频率。根据所述步骤2)中X轴第i个插补周期需要发送的脉冲数为M_Xi和所述步骤3)中X轴第i个插补周期更新为T′_Xi，可得X轴在第i个插补周期的脉冲发送频率为f_Xi：

f_Xi＝M_Xi/T′_Xi (10)

同理可得，Y轴在第i个插补周期的脉冲发送频率为f_Yi：

f_Yi＝M_Yi/T′_Yi (11)

所述同步误差补偿方法还包括以下步骤：

4)在运动过程中，将参考轴X轴每次插补周期发送的脉冲数进行累加判断是否运动到终点。

将X轴每次插补周期发送的脉冲数进行累加得到M_Xadd：

M_Xadd＝M_X0+M_X1+M_X2+...+M_Xi (12)

所述步骤1)中X轴加速长度为L_Xacce、匀速长度为L_Xcons、减速长度为L_Xdece，在加速过程中，所述公式(1)和所述公式(2)中的a_j大于0，当M_Xadd≥L_Xacce时，说明加速过程完成进入匀速过程；在匀速过程中，所述公式(1)和所述公式(2)中的a_j恒为0，当M_Xadd≥L_Xacce+L_Xcons时，提前进入减速过程以确保减速到终止速度v_Xl；在减速过程中，所述公式(1)和所述公式(2)中的a_j小于0，当M_Xadd≥L时，以终止速度v_Xl运行M_Xi-(M_Xadd-L)个脉冲达到线段的终点。

Claims (4)

1.一种两轴联动机械臂运动控制的同步误差补偿方法，其特征在于：所述方法包括以下步骤：

1)采用S型加减速算法并结合电机的性能，建立基于频率的速度表(v₀,v₁,v₂,...,v_n)，基于脉冲数的步长表(s₀,s₁,s₂,...,s_n)和基于步长的加速度表(a₀,a₁,a₂,...,a_n)，三者间的关系有a_i＝(v_i+1-v_i)/s_i；

设在坐标系中有一线段l，其长度为L，与X轴夹角为θ，线段l在X轴上的映射长度大于在Y轴上的映射长度，选取X轴为参考轴，结合速度规划得到起始速度v_start、终止速度v_end和匀速速度v_cons，则由起始速度v_start得X轴起始速度v_X0＝v_start*cosθ，Y轴起始速度v_Y0＝v_start*sinθ，同理可得，X轴终止速度v_Xl，Y轴终止速度v_Yl，X轴匀速速度v_Xn，Y轴匀速速度v_Yn，计算X轴加速长度为L_Xacce、匀速长度为L_Xcons和减速长度为L_Xdece，则L＝L_Xacce+L_Xcons+L_Xdece；

2)在运动过程中进行同步插补，计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数；

计算X轴在第i+1个插补周期的速度v_Xi+1：

v_Xi+1＝v_Xi+a_j*M_Xi (1)

其中，v_Xi为X轴第i个插补周期的速度，加速度a_j的下标j为v_Xi在速度表中(v₀,v₁,v₂,...,v_n)的位置，通过搜索v_j≤v_Xi＜v_j+1来确定下标j,M_Xi为X轴第i个插补周期需要发送的脉冲数；

同理可得，Y轴第i+1个插补周期的速度v_Yi+1：

v_Yi+1＝v_Yi+a_j*M_Yi (2)

所述公式(1)和公式(2)中M_Xi、M_Yi的确定方法如下：

计算X轴第i个插补周期需要发送的脉冲数M_Xi：

M_Xi＝int[T*v_Xi] (3)

其中，int[]表示对计算结果进行取整，确保得到的脉冲数为整数；T为固定的插补时间，由系统的插补精度要求和硬件的相关参数共同决定；v_Xi为X轴第i个插补周期的速度，则X轴第i个插补周期T_Xi为：

T_Xi＝M_Xi/v_Xi (4)

再计算Y轴第i个插补周期需要发送的脉冲数M_Yi：

M_Yi＝int[T_Yi*v_Yi] (5)

其中，T_Yi为Y轴第i个插补周期，T_Yi＝T_Xi+ΔT_Yi-1，ΔT_Yi-1为Y轴上一次插补周期计算M_Yi-1取整时舍去的小数所对应的时间，v_Yi为Y轴第i个插补周期的速度，计算T_Yi*v_Yi取整时舍去的小数所对应的时间ΔT_Yi，加到下一次Y轴的插补周期T_Yi+1中；

3)使用伺服器的编码器进行同步误差反馈补偿，根据伺服器编码器实时反馈的位置来确定X轴、Y轴的同步脉冲误差ΔM_Xi、ΔM_Yi和同步时间误差Δt_Xi、Δt_Yi。

2.如权利要求1所述的两轴联动机械臂运动控制的同步误差补偿方法，其特征在于：在X轴第i个插补周期结束时，采集编码器反馈回来的当前插补周期内X轴运行的脉冲数M'_Xi，计算X轴在第i个插补周期内的同步脉冲误差ΔM_Xi和同步时间误差Δt_Xi：

ΔM_Xi＝M_Xi-M'_Xi (6)

Δt_Xi＝ΔM_Xi/v_Xi (7)

同理可得，Y轴在第i个插补周期内的同步脉冲误差M'_Yi和同步时间误差Δt_Yi：

ΔM_Yi＝M_Yi-M'_Yi (8)

Δt_Yi＝ΔM_Yi/v_Yi (9)

若Δt_Xi＞Δt_Yi，说明X轴滞后Y轴，则Y轴第i+1个插补周期更新为T'_Yi+1，即T′_Yi+1＝T_Yi+1+(Δt_Xi-Δt_Yi),X轴第i+1个插补周期保持不变，即T′_Xi+1＝T_Xi+1；若Δt_Xi＜Δt_Yi，说明Y轴滞后X轴，则X轴第i+1个插补周期更新为T'_Xi+1，即T'_Xi+1＝T_Xi+1+(Δt_Yi-Δt_Xi)，Y轴第i+1个插补周期保持不变，即T′_Yi+1＝T_Yi+1。

3.如权利要求1或2所述的两轴联动机械臂运动控制的同步误差补偿方法，其特征在于：计算X轴、Y轴每个插补周期的脉冲发送频率，根据所述步骤2)中X轴第i个插补周期需要发送的脉冲数为M_Xi和所述步骤3)中X轴第i个插补周期更新为T′_Xi，得X轴在第i个插补周期的脉冲发送频率为f_Xi：

f_Xi＝M_Xi/T′_Xi (10)

同理可得，Y轴在第i个插补周期的脉冲发送频率为f_Yi：

f_Yi＝M_Yi/T′_Yi (11)。

4.如权利要求2所述的两轴联动机械臂运动控制的同步误差补偿方法，其特征在于：所述同步误差补偿方法还包括以下步骤：

4)在运动过程中，将参考轴X轴每次插补周期发送的脉冲数进行累加判断是否运动到终点；

将X轴每次插补周期发送的脉冲数进行累加得到M_Xadd：

M_Xadd＝M_X0+M_X1+M_X2+...+M_Xi (12)

所述步骤1)中X轴加速长度为L_Xacce、匀速长度为L_Xcons、减速长度为L_Xdece，在加速过程中，所述公式(1)和所述公式(2)中的a_j大于0，当M_Xadd≥L_Xacce时，说明加速过程完成进入匀速过程；在匀速过程中，所述公式(1)和所述公式(2)中的a_j恒为0，当M_Xadd≥L_Xacce+L_Xcons时，提前进入减速过程以确保减速到终止速度v_Xl；在减速过程中，所述公式(1)和所述公式(2)中的a_j小于0，当M_Xadd≥L时，以终止速度v_Xl运行M_Xi-(M_Xadd-L)个脉冲达到线段的终点。