CN109407621A - 一种取样机械臂插补中s型加减速运动控制方法 - Google Patents

Abstract

本发明公开了一种取样机械臂插补中S型加减速运动控制方法，包括步骤：a)上位机接收用户操作指令，设置取样机械臂插补中S型加减速运动的轨迹规划方式和对应的运动参数，将轨迹规划方式和运动参数发送至取样机械臂运动控制器；b)取样机械臂的运动控制器根据轨迹规划方式对应的算法对取样机械臂的实际运动轨迹进行运算，得到取样机械臂的实际运动轨迹信息，并将实际运动轨迹信息发送至取样机械臂；c)取样机械臂接收实际运动轨迹信息，根据实际运动轨迹信息进行运动。本发明的取样机械臂插补中S型加减速运动控制方法融合插补算法和S型加减速算法，实现了机械臂最优路径两轴联动，同时可平稳高速运动，机械臂定位精度和工作效率显著提高。

Description

一种取样机械臂插补中S型加减速运动控制方法

技术领域

本发明涉及机械手控制技术领域，具体涉及一种取样机械臂插补中S型加减速运动控制方法。

背景技术

目前，国内机械臂行业发展迅速，控制器的种类繁多。然而，对于沉降式液基薄层细胞制片机的取样机械臂，需要按制片流程正确转移试剂和样品，并对样品进行混匀打散等操作，这些过程需要非常准确的完成。因此，取样机械臂的控制过程是一个高速度、高精度的定位过程。高速度指的是取样控制***要具有好的瞬态特性，也就是说***的响应速度要快；高精度的含义是***的稳态误差要小。所以，高速度、高精度就是要求***的响应要“快、准、稳”。

要设计和实现高速度、高精度的取样平台控制***，首先要设计合理的结构，选用合适的运动控制元件。其次要使用适当的运动规划算法，提高***响应精度。而运动控制硬件和算法实现需结合功能、需求和成本各方面综合考虑。随着电子技术发展，嵌入式单片机功能和处理能力不断提升，虽然插补运动、加减速控制等算法计算量较大、需要的硬件支持较多，但经过合理规划和优化，现单片机上也足以实现这些算法，已足够胜任作为控制核心。

现有技术中插补算法可实现最优路径两轴联动，加减速算法则实现机械平稳高速运动，然而在沉降式液基薄层细胞制片机实际操作过程中，取样机械臂需要同时满足最优路径多轴联动且又快又稳操作。因此，亟待开发出一种插补中S型加减速运动控制方法。

发明内容

针对现有技术中存在的技术问题，本发明的目的在于提供了一种取样机械臂插补中S型加减速运动控制方法，有效融合插补算法和S型加减速控制方法，实现了机械臂最优路径两轴联动，同时可平稳高速运动，机械臂定位精度和工作效率均显著提高。

本发明的技术方案是这样实现的：

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

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

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

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

进一步地，所述步骤a)中插补方式包括直线插补和圆弧插补。

更进一步地，当插补方式是直线插补时，则运动参数包括起点坐标、终点坐标和插补精度值；当插补方式是圆弧插补时，则运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向。

更进一步地，所述直线插补的步骤为：

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

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

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

更进一步地，所述圆弧插补的步骤为：

(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]的正整数。

进一步地，所述步骤a)中轨迹规划方式包括插补前S型加减速控制方式和插补后S型加减速控制方式。

更进一步地，所述步骤a)中轨迹规划方式为插补前S型加减速控制方式。

进一步地，所述步骤b)包括以下步骤：

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

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

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

与现有技术相比，本发明的有益效果在于：

(1)本发明公开的一种取样机械臂插补中S型加减速运动控制方法，有效融合插补算法和S型加减速控制方法，实现了机械臂最优路径两轴联动，同时可平稳高速运动，机械臂定位精度和工作效率均显著提高。

(2)本发明公开的一种取样机械臂插补中S型加减速运动控制方法实现了机械臂两轴联动，工作效率大大提高。

(3)本发明的控制算法将运动参数预先模拟好保存入数据表中，用查表法减少计算量，效率高，功能丰富。

附图说明

图1为实施例1中DDA直线插补器的工作流程图；

图2为实施例1中DDA直线插补软件流程图；

图3为实施例2中电机矩频特征曲线；

图4为实施例2中实际运行过程中S形加减速曲线示意图；

图5为实施例2中实际运行过程中S形加减速曲线的解释示意图；

图6为实施例2中步进电机加速阶段曲线离散化示意图；

图7为实施例3中插补前S型加减速控制方案示意图。

图8为实施例4中工位示意图；

图9为实施例4中减速曲线验证实验中实测电机频率曲线1示意图；

图10为实施例4中减速曲线验证实验中实测电机频率曲线2示意图。

具体实施方式

展示一下实例来具体说明本发明的某些实施例，且不应解释为限制本发明的范围。对本发明公开的内容可以同时从材料、方法和反应条件进行改进，所有这些改进，均应落入本发明的的精神和范围之内。

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

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

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

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

本实施例中，所用的插补方式包括直线插补和圆弧插补，当插补方式是直线插补时，则运动参数包括起点坐标、终点坐标和插补精度值；当插补方式是圆弧插补时，则运动参数包括起点坐标、终点坐标、圆心坐标、圆弧插补精度值及圆弧插补方向。其中，直线插补的方法：(1)为取样机械臂的运动控制器获取运动参数中的起点坐标(X0，Y0)、终点坐标(Xe，Ye)及插补精度值ΔL，并分别根据ΔX＝Xe-X0和ΔY＝Ye-Y0得到终点坐标与起点坐标的横向增量ΔX及纵向增量ΔY；(2)将横向增量ΔX及纵向增量ΔY的绝对值输入寄存器中，各轴的方向标志位根据增量值的正负来设置；(3)根据DDA插补算法计算X轴和Y轴脉冲数和脉冲组合。圆弧插补的方法为：(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]的正整数。

本发明实施例中，将插补算法和S型加减速控制方法有效融合，从而控制机械臂最优路径两轴联动，同时可平稳高速运动，是采用插补前S型加减速控制方式，具体方法为：b1)采用S型加减速算法并结合电机的性能，建立基于S曲线的加减速频段表，并将加减速频段表转换为单片机定时器对应初值的常数表；然后将定时器初值表加载到单片机定时器模块寄存器，并在脉冲中断子程序中实时记录脉冲数，当其到达本频段脉冲数时则切换到下一频段，直至加载完所有频段，完成指定运动；b2)运行过程中，在定时器未进中断前的空闲时间利用DDA插补算法计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数，确定中断下一次发送脉冲的组合；b3)单片机脉冲中断依据步骤b2)确定的脉冲组合发送脉冲，控制取样机械臂操作。

实施例1：DDA插补算法确定X轴、Y轴脉冲组合

取样机械臂在取样过程中通过X轴、Y轴以及Z轴的联动，在各组之间和各工位之间做快速、往复运动。其中X轴、Y轴的运动实现了取样机械臂在平面上的定位和特殊动作(如圆周搅拌运动)，Z轴使得机械臂运动到目标点后，做垂直上下运动，进行取样品、试剂的动作。

平面两点之间运动控制最具效率的运动规划是计算出两点间的最短直线轨迹，通过直线插补的办法按轨迹两轴联动同步移动到目标点，因此直线插补是实现***高速度、高精度要求的一个重要途径。

插补是指数据密化的过程，是在己知起点或终点有限坐标的情况下，计算机运用适当的算法，根据线段的直线或圆弧等特点，在有限坐标点之间生成一系列中间点坐标数据，从而自动完成整个线段的轨迹运算。根据积分的基本原理，从几何意义上讲，函数x＝f(t)的在t₀～t_n区间的积分，就是该函数曲线与横坐标t在区间t₀～t_n所围成的面积。如果将时间区间t₀～t_n划分为间隔△t的很多小区间，并且当△t足够小时，函数的积分还可以看作是一系列的小矩形面积之和。可以用下式表示：

当取△t为基本单位1时，上式可以演化为：

以X轴为基轴建立平面直角坐标系。设坐标原点O为运行的起点，直线的运行终点为E(Xe，Ye)。可将问题简化，设Xe＞Ye，且Xe、Ye均为正值。利用已知的进给速度V根据积分原理，其微小位移量为：

ΔX＝V_x.Δt

ΔY＝V_y·Δt

对于直线，存在以下的比例关系：

从而坐标轴的位移增量为：

ΔX＝KX_eΔt

ΔY＝KY_eΔt

将上式积分，取△t_i＝1，得到各个坐标轴的位移量分别为：

可见，经过n次累加后，机械臂正好到达终点E(Xe，Ye)。由于***中相应的寄存器的容量限制式中Xe，Ye的最大允许值，因此对于容量为N位的寄存器，需要选取K＝1/2^N，使得坐标轴每次分配的进给脉冲不会超过一个脉冲当量，而且Xe，Ye不会超过寄存器容量范围。这样累加次数为：n＝1/K＝2^N。当一个脉冲信号△t由脉冲源发出后，每个X,Y积分器各累加一次，当累加结果超出2^N时，就会溢出一个脉冲△X(或△Y)，这样经过2^N次的累加之后，每个坐标轴的脉冲总数就等于该坐标的被积函数，使得机械臂能够准确到达终点E(Xe，Ye)。应用于直线插补的插补器如图1所示。

实现DDA插补算法程序流程图如图2所示。对于N位寄存器，直线插补累加2^N次才能到达终点。因此可以用一容量为2^N的寄存器当计数器来统计累加的次数。可以用加1计数器，也可以用减1计数器。用加1计数器时，首先将计数器清零，运算过程中每来一个脉冲△t就累加1。当计数器满2^N时表明运算完成。采用减1计数器时，运算前把总运算次数2^N送入计数器，每运算一次，就减去1。当计数器减为0时，表明运算完成。故终点判别为J＝2^N。对于其它象限的插补运算，可进行简化处理。即进入插补程序之前，将两坐标增量的绝对值存入J_VX和J_VY寄存器中，各轴的方向标志位根据增量值的正负来设置。程序输出进给脉冲时，要根据标志位的状态输出正负方向的进给脉冲。

使用单片机C语言较容易实现DDA插补算法，计算量不大且实时性好。圆弧插补实现原理与直线插补类似。

实施例2：步进电机加减速控制方法

本实施例中采用S曲线加减速控制实现步进电机升降速控制。S曲线加减速控制，根据实际情况，加速过程采用慢、快、慢的过程来控制步进电机，减速过程正好相反。适用于平稳性要求较高的场合。该项目采用s形加减速，处理速度很快，稳定性也较好，并能准确定位。

(1)步进电机特性和S形曲线分析

由图3步进电机的矩频特性曲线可以看出，当步进电机转速升高时，输出转矩会随之呈非线性下降，而且在高速阶段下降速度会更快。这个转矩实际上是电磁转矩T₀和整个***中影响运行的阻力矩T_L的差。

设步进电机的阻力矩T_L＝KTω。负载转矩是加速转矩Jω，则负载的运动方程为：

Jω＝T₀-K_Tω

式中：T₀为电磁转矩，J为负载的转动惯量，K_T为转矩常数，ω为步进电机的回转角速度。假设电机从速度为0开始加速，即t₀＝0，ω＝0时，由上式可知：

式中：τ＝J/KT，如果驱动脉冲的频率为f，由ω＝Kf，可以得出f＝ω/K，则

f(t)＝f_m-f_me^-t/τ

式中：fm＝T₀/KK_T为步进电机运行的最高连续频率，τ是决定步进电机升速快慢的时间常数，它的值要根据实验来确定，这个值也是步进电机升降速过程控制的关键。驱动频率f是随着时间t呈指数规律上升，因此指数规律的升降速曲线更能使步进电机的转子的角加速度的变化和输出转矩的变化相适应。在实际运行中，因为步进电机启动时，由于其静态惯性比较大，须以较小的加速度加速；而在较高速度运行时，由于输出转矩会有所降低，须以较小的加速度升降速；在步进电机停止时，为了防止发生过冲，须以较小的加速度降速，因此实际运行情况按图4所示的S形曲线进行升降频。

S曲线加减速的特征是存在加加速度，故加速度无突变，在速度变换过程中不存在柔性冲击，适合高速运动。完整的S曲线加减速过程如图5所示，共分5个阶段。以拐点时刻t₁为分界点，在0～t₁时间段内，速度由起始速度V₀加速至拐点速度V₁，加加速度a’为常数c，加速度a由0逐渐增加；t₁～t₂时间段内，速度由V1加速至运行速度Vt，a’为负值，a逐渐减小至0；t₂～t₃为匀速段，保持速度V₁不变；t₃～t₄段、t₄～t₅为减速段，与加速段类似，只是加速度a为负值。

(2)步进电机加减速频段表

将连续的加减速曲线转化为电脉冲信号的基本方法是将曲线离散到各个频段，在内存中生成频段表，然后查表依次加载各个频段，使控制***实际产生的脉冲曲线拟合理论加减速曲线。加速阶段曲线离散如图6所示。离散实现的具体方法为将整个加速时间等分为若干时间段，每个时间段的长度固定为t’，一般取t’＝2/f_startt，其中f_startt为电机的起始频率。然后将时间t从t₀开始以t’为跨度依次递增并计算每个t时刻处的曲线理论值，此值即为每个频段的频率值。减速阶段可以看作加速阶段的逆过程，二者离散实现过程基本一致，不同点在于减速阶段起始频率换作停止频率。

将频段表转换为单片机定时器对应初值的常数表，然后将定时器初值表加载到单片机定时器模块寄存器，并在脉冲中断子程序中实时记录脉冲数，当其到达本频段脉冲数时则切换到下一频段，直至加载完所有频段，完成指定运动。

实施例3：插补中S型加减速控制方法

本实施例中要求用于规划最短的移动距离，而圆弧插补目的是做特殊圆周搅拦动作，因此对插补轨迹误差精度要求不高，但位置精度要求高，所以采用的是插补前S型加减速控制方式，控制方案如图7所示。

插补前S型加减速控制即插补计算前进行加减速运算，先在加减速运算中计算出下一次插补的时机，时机到后进行插补计算并输出各个轴的分量。它的优点是在不影响位置精度的情况下控制合成速度，但需要预测减速点。对于高速高精加工中的曲面或曲线插补，需要考虑到弓高误差和许用加速度。因此对于直线大都采用位移累积比较的方法来预测减速点；对于圆弧采用角度累积比较的方法来预测减速点。这两类型线较简单，直线的曲率为零，无需考虑弓高误差，同一段圆弧的曲率是恒值，所以可以预先确定弓高误差和加速度。

运行过程中，在定时器未进中断前的空闲时间利用DDA插补算法计算X轴、Y轴每个插补周期的速度和需要发送的脉冲数，确定中断下一次发送脉冲的组合；但此时不发送脉冲，此时设置一个互斥等待信号，等待定时器从进入中断时将脉冲发出，中断出去时打开互斥信号通知插补算法进行下一次计算，循环用互斥信号控制脉冲发送间隔，这样就实现了简单的插补中S型加减速控制方法。该方法计算量小，占用资源少，非常适合在单片机***中运行。因为发送脉冲有时是01或10组合，每个轴不是连续的，导致调速S曲线的拟合会略有偏差，但脉冲数不会丢失，能保证插补的精度，而这正是本***的实际需求。

实施例4：加减速曲线验证实验

1.运动效率对比实验

插补运动是两轴联动，工作效率当然比单轴运行高，这里主要测试提高效率的程度和水平。选择了几个常用的需频繁定位的工位，首先用常规单轴直角路线法移动，然后用直线插补法移动，分别记录运行时间，两者进行对比并查看效率差。最后分别用两种方法运行完成整个制片工作流程，进行时间对比。选择的几个工位如图8所示，标示的几个位置是操作比较多的工位，实际运行中需要频繁定位，沿坐标轴方向(如1→2,1→3)两种运行方式无明显区别，因此选择了斜线路线(如1→4,1→B)作对比。测试结果见表1。

表1两种运动方式效率对比

2.步长距离标定实验

本实施例测量的位移范围(0-49cm)，选择广州诺信KA300-570型光栅尺，有效范围为0-57cm，XY轴各安装一根。数据采集卡为其配套的SDC-4采集卡，可自动记录保存当前坐标值。

机械臂的直线运动由同步带传动，同步带轮型号为XL20，节径d＝32.34mm，因此其每周传动距离即导程为32.34*π＝101.55mm。步进电机步距角为1.8°，细分数为4细分，故每周所需脉冲数为200*4＝800步。因此步进电机的步长距离理论计算值为0.1293mm。由于同步轮加工工艺和装配工艺等原因，实际结果与理论计算值会有偏差。测量每轴的定位精度时，应该首先排除同步轮带来的误差，因此先测量标定出每轴的实际步长距离。为保证电机传动精确，在启动和停止时不丢步不堵转，所有测试都是在电机以启动速度匀速运行状态下测得。在整个行程内从不同起始位置记录电机旋转一周的传动距离，光栅尺记录始末位置坐标值。X、Y轴测量结果分别如表2和表3。可知，实际标定的X轴步长距离为0.1284mm，Y轴步长距离为0.1298mm，与理论值(0.1293mm)存在一定偏差，应以实际标定结果作为后续计算测试的依据。从测试结果还可看出，每轴固定发送800个脉冲的情况下，其定位偏差均不超过0.2mm，精度范围已经可以满足使用要求。

表2X轴步长距离标定实验

表3Y轴步长距离标定实验

3.定位精度测试实验

下面的实验用于测试机械臂的插补定位精度。在工作区域内机械臂从起点插补运动到终点，其起点位置由手动调节，以光栅尺作为标尺，为方便调节和记录，起点位置选在对角线附近。目标坐标为随机选择。由于程序进行插补时要将实际坐标偏差转成脉冲偏差，根据坐标偏差，每个轴除以该轴的步长距离即为计算的理论脉冲数，插补时就以此脉冲数做为目标。插补完成后光栅尺测量实际坐标与目标坐标进行对比，计算实际插补运动定位精度。结果见表4，表中负号表示方向，即反向运动。最终测得10次结果中各轴理论计算值和实际定位值偏差都未超过0.2，说明定位精度也满足要求。与单轴匀速运动测量的数据对比可知，插补算法也不会影响定位精度，具有很高的可靠性。

表4平面插补定位精度测试

4.减速曲线验证实验

已知同步带轮传动的导程为101.55mm，工作区域约为450mm*300mm，预设2-3s走完整个行程，即电机平均转速为(450mm/101.55mm)/2.5s＝1.7r/s＝102rpm，步进电机800个脉冲转一周，故电机平均工作频率＝102*800/60＝1360Hz。加减速S曲线设计时根据预估值范围预设，最终确定为启动频率为150Hz，运行频率为1500Hz。

为了验证步进电机的实际运行频率变化是否符合S曲线，采用光电编码器结合单片机的计时器TC模块的捕捉模式作为测试***。编码器通过联轴器连接步进电机尾轴同步转动，其发出的脉冲作为输入信号传送至单片机的TIOB引脚。在捕捉模式下，当TIOB引脚检测到脉冲上升沿时TC计数器值会被装载至TIOB寄存器并触发中断，计数器同时被清零，通过在中断服务程序中读取TIOB寄存器值即可获取脉冲间隔。从电机从启动至停止过程中每隔0.1s采样读取TIOB寄存器值N，首先让电机运行一个较长的距离保证加减速过程做完，测试结果如表5；之后运行一个较短的距离测试裁剪的加减速曲线，测试结果如表6所示。

表5寄存器脉冲采样记录1

表2寄存器脉冲采样记录2

因测试***TC模块的时钟频率f_clock为24MHz，寄存器值除以时钟频率即为实际时间间隔，将时间间隔取倒数即得到各时刻处频率值f＝f_clock/N。根据上式将各时刻寄存器记录值转换为频率值并分别绘制成曲线，如图9及图10所示。

由图9可以看出，步进电机的加速过程与减速过程均符合S曲线规律，电机加速到最高频率后就开始恒速运动并预估好减速点开始减速，编码器发出的脉冲频率间隔切换平滑，电机运行平稳，说明插补计算对单轴调速曲线无大影响；图10是运行距离较短时的曲线，可以看出控制***根据实际运行距离要求，将电机加速到1200Hz左右便开始减速，缩短了加减速时间，提高了效率。这里仅展示了X轴测试结果，Y轴结果与此类似，两者都与设计的曲线比较吻合，达到了预期的控制效果。

本发明的取样机械臂插补中S型加减速运动控制方法，有效融合插补算法和S型加减速控制方法，实现了机械臂最优路径两轴联动，同时可平稳高速运动，机械臂定位精度和工作效率均显著提高；本发明的控制算法将运动参数预先模拟好保存入数据表中，用查表法减少计算量，效率高，功能丰富。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (8)

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)确定的脉冲组合发送脉冲，控制取样机械臂操作。