[发明专利]一种取样机械臂插补中S型加减速运动控制方法

权利要求书

1.一种取样机械臂插补中S型加减速运动控制方法，其特征在于，所述方法包括以下步骤：

a)上位机接收用户的操作指令，设置取样机械臂的轨迹规划方式以及与所述轨迹规划方式对应的运动参数，并将所述轨迹规划方式和所述运动参数发送至取样机械臂的运动控制器；所述轨迹规划方式为插补与加减速控制结合的方式；

b)所述取样机械臂的运动控制器根据所述轨迹规划方式对应的算法对取样机械臂的实际运动轨迹进行运算，得到取样机械臂的实际运动轨迹信息，并将所述实际运动轨迹信息发送至取样机械臂；

c)所述取样机械臂接收所述实际运动轨迹信息，并根据实际运动轨迹信息进行运动。

2.根据权利要求1所述的一种取样机械臂插补中S型加减速运动控制方法，其特征在于，所述步骤a)中插补方式包括直线插补和圆弧插补。

3.根据权利要求2所述的一种取样机械臂插补中S型加减速运动控制方法，其特征在于，当插补方式是直线插补时，则运动参数包括起点坐标、终点坐标和插补精度值；当插补方式是圆弧插补时，则运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向。

4.根据权利要求2所述的一种取样机械臂插补中S型加减速运动控制方法，其特征在于，所述直线插补的步骤为：

(1)取样机械臂的运动控制器获取运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向增量ΔX及纵向增量ΔY；

(2)将横向增量ΔX及纵向增量ΔY的绝对值输入寄存器中，各轴的方向标志位根据增量值的正负来设置；

(3)根据DDA插补算法计算X轴和Y轴脉冲数和脉冲组合。

5.根据权利要求2所述的一种取样机械臂插补中S型加减速运动控制方法，其特征在于，所述圆弧插补的步骤为：

(1)取样机械臂的运动控制器获取所述运动参数中的起点坐标(Xa，，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)、圆弧插补精度值DfStep及圆弧插补方向；

(2)根据起点坐标(Xa，Ya)、终点坐标(Xb，Yb)、圆心坐标(Xo，Yo)及圆弧插补方向确定半径R、起点角度φa、终点角度φb、及终点角度与起点角度之差Δφ，并根据N＝floor(|π*R*Δφ/(180*DfStep)|+1)计算圆弧插补次数N，并根据Dfstep＝Δφ/N计算当前圆弧插补精度Dfstep，根据Xm＝R*cos(φa+m*Dfstep)+Xo确定实际运动轨迹中N个插补点的横坐标，根据Ym＝R*sin(φa+m*Dfstep)+Yo确定实际运动轨迹中N个插补点的纵坐标；其中，floor函数为向下取整函数，m为取值范围为[1,N]的正整数。

6.根据权利要求1所述的一种取样机械臂插补中S型加减速运动控制方法，其特征在于，所述步骤a)中轨迹规划方式包括插补前S型加减速控制方式和插补后S型加减速控制方式。

7.根据权利要求6所述的一种取样机械臂插补中S型加减速运动控制方法，其特征在于，所述步骤a)中轨迹规划方式为插补前S型加减速控制方式。

8.根据权利要求1所述的一种取样机械臂插补中S型加减速运动控制方法，其特征在于，所述步骤b)包括以下步骤：

b1)采用S型加减速算法并结合电机的性能，建立基于S曲线的加减速频段表，并将加减速频段表转换为单片机定时器对应初值的常数表；然后将定时器初值表加载到单片机定时器模块寄存器，并在脉冲中断子程序中实时记录脉冲数，当其到达本频段脉冲数时则切换到下一频段，直至加载完所有频段，完成指定运动；

b2)运行过程中，在定时器未进中断前的空闲时间利用DDA插补算法计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数，确定中断下一次发送脉冲的组合；

b3)单片机脉冲中断依据步骤b2)确定的脉冲组合发送脉冲，控制取样机械臂操作。