[发明专利]一种基于CANOPEN协议控制伺服电机曲线运动的方法

权利要求书

1.一种基于CANOPEN协议控制伺服电机曲线运动的方法，实现多台伺服电机带动工件完成曲线运动，所述CANOPEN协议包括主站和从站，其特征在于，控制各伺服电机运动的多台伺服驱动器中，作为主站的为主控制器，作为从站的为从控制器，所述基于CANOPEN协议控制伺服电机曲线运动的方法，包括：

步骤S1、主站检测当前CAN总线中在线的从站；

步骤S2、主站发送广播指令，通知各在线从站进入预操作模式；

步骤S3、主站发送TPDO指令，配置各在线从站的通信参数和映射参数，并且发送广播指令，通知各在线从站控制伺服电机运动至零点位置并进入操作模式；

步骤S4、主站接收到位移指令后，解析得到位移指令中包含的目标曲线运动轨迹，将目标曲线运动轨迹离散化为若干个小线段，依次取小线段作为当前理想运动轨迹；

步骤S5、主站发送周期性SYNC指令，根据各从站的反馈得到工件的当前位置信息(x_i，y_i)，其中i表示当前周期，并根据当前位置信息(x_i，y_i)计算第一预测位置(x_i+D₀，y_i)和第二预测位置(x_i+D₀，y_i+D₀)在竖直方向上和当前理想运动轨迹的差值，其中D₀为移动距离，取最小差值对应的预测位置作为下一周期的目标位置，并通过TPOD指令将目标位置拆分发送给对应的在线从站，直至工件运动到当前理想运动轨迹的末端；

步骤S6、判断当前理想运动轨迹是否为目标曲线运动轨迹中的最后一个小线段，若是则完成并结束；若不是则取下一个小线段作为当前理想运动轨迹，并重新执行步骤S5；

其中，所述步骤S5具体包括以下步骤：

S5.1、设当前理想运动轨迹的轨迹方程为y＝kx+b，主站发送SYNC指令获取各在线从站反馈的位置，合并得到工件的当前位置信息(x_i，y_i)，i≥1；

S5.2、令每个周期内工件在X轴上的移动距离为D₀，在Y轴上的移动距离为0或D₀，因此得到下一周期工件的两个预测位置分别为：第一预测位置(x_i+D₀，y_i)和第二预测位置(x_i+D₀，y_i+D₀)；

S5.3、根据轨迹方程得到下一周期工件的轨迹位置为(x_i+1′，y_i+1′)，且Y_i+1y_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀，其中i+1表示下一周期；

S5.4、分别计算预测位置与轨迹位置之间的距离：

d₁＝|y_i+1′-y_i|＝|kx_i+1′+b-y_i|

d₂＝|y_i+D₀-y_i+1′|＝|y_i+D₀-kx_i+1′-b|

S5.5、根据距离d₁、d₂确定下一周期的目标位置(x_i+1，y_i+1)，包括：

若d₁＞d₂时，表明第二预测位置(x_i+D₀，y_i+D₀)距离当前理想运动轨迹较近，则选取第二预测位置作为下一个周期的目标位置，即下一个周期的目标位置(x_i+1，y_i+1)中x_i+1＝x_i+D₀，y_i+1＝y_i+D₀；

当d₁≤d₂时，表明第一预测位置(x_i+D₀，y_i)距离当前理想运动轨迹较近，则选取第一预测位置作为下一个周期的目标位置，即下一个周期的目标位置(x_i+1，y_i+1)中x_i+1＝x_i+D₀，y_i+1＝y_i；

S5.6、主站通过TPOD指令将目标位置拆分发送给对应的在线从站，控制从站下一周期的运动距离，使工件到达下一周期的目标位置；

S5.7、重复执行步骤S5.1～步骤S5.6，直至工件运动到当前理想运动轨迹的末端；

或者，所述步骤S5具体包括以下步骤：

S5.1、设当前理想运动轨迹的轨迹方程为y＝kx+b，主站发送SYNC指令获取各在线从站反馈的位置，合并得到工件的当前位置信息(x_i，y_i)，i≥1；

S5.2、令每个周期内工件在X轴上的移动距离为D₀，在Y轴上的移动距离为0或D₀，因此得到下一周期工件的两个预测位置分别为：第一预测位置(x_i+D₀，y_i)和第二预测位置(x_i+D₀，y_i+D₀)；

S5.3、根据轨迹方程得到下一周期工件的轨迹位置为(x_i+1′，y_i+1′)，且y_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀，其中i+1表示下一周期；

S5.4、分别计算预测位置与轨迹位置之间的Y轴位置差：

d₃＝y_i+1′-y_i＝kx_i+1′+b-y_i

d₄＝y_i+D₀-y_i+1′＝y_i+D₀-kx_i+1′-b

S5.5、根据距离d₃、d₄确定下一周期的目标位置(x_i+1，y_i+1)，包括：

令C_i＝Δx×(d₃-d₄)＝2Δy×x_i-2Δx×y_i+f

其中，Δx＝(x₂-x₁)，Δy＝(y₂-y₁)，(x₁，y₁)和(x₂，y₂)为当前理想运动轨迹上的第一个点和第二个点，且k＝Δy/Δx，f＝2Δy×D₀+Δx(2b-D₀)；

对于第i+1个周期而言，C_i+1＝Δx×(d₃-d₄)＝2Δy×x_i+1-2Δx×y_i+1+f，且x_i+1＝x_i+D₀；

将两式相减得到递推公式为：C_i+1＝C_i+2Δy×D₀-2Δx(y_i+1-y_i)；

假设工件的起始运动点为当前理想运动轨迹上的第一个点，则满足：y₁＝kx₁+b；

因此C₁＝2Δy×D₀-Δx×D₀；

根据C_i的计算公式可知，C_i与d₃-d₄具有相同的符号，因此可得如下判定规则：若k＞0，且C_i≥0时，表明第二预测位置(x_i+D₀，y_i+D₀)距离当前理想运动轨迹较近，则选取第二预测位置作为下一个周期的目标位置，即下一个周期的目标位置(x_i+1，y_i+1)中x_i+1＝x_i+D₀，y_i+1＝y_i+D₀，并且根据递推公式可得C_i+1＝C_i+2D₀(Δy-Δx)；

当k＞0，且C_i＜0时，表明第一预测位置(x_i+D₀，y_i)距离当前理想运动轨迹较近，则选取第一预测位置作为下一个周期的目标位置，即下一个周期的目标位置(x_i+1，y_i+1)中x_i+1＝x_i+D₀，y_i+1＝y_i，并且根据递推公式可得C_i+1＝C_i+2Δy×D₀；

若k＜0，且C_i＜0时，表明第二预测位置(x_i+D₀，y_i+D₀)距离当前理想运动轨迹较近，则选取第二预测位置作为下一个周期的目标位置，即下一个周期的目标位置(x_i+1，y_i+1)中x_i+1＝x_i+D₀，y_i+1＝y_i+D₀，并且根据递推公式可得C_i+1＝C_i+2D₀(Δy-Δx)；

当k＞0，且C_i≥0时，表明第一预测位置(x_i+D₀，y_i)距离当前理想运动轨迹较近，则选取第一预测位置作为下一个周期的目标位置，即下一个周期的目标位置(x_i+1，y_i+1)中x_i+1＝x_i+D₀，y_i+1＝y_i，并且根据递推公式可得C_i+1＝C_i+2Δy×D₀；

S5.6、主站通过TPOD指令将目标位置拆分发送给对应的在线从站，控制从站下一周期的运动距离，使工件到达下一周期的目标位置；

S5.7、重复执行步骤S5.1～步骤S5.6，直至工件运动到当前理想运动轨迹的末端。