基于三轴运动模组的具有避障和等待位的运动控制方法

贺长林，殷 杰，徐其航

（1.中电鹏程智能装备有限公司，南京 210008；2.邵东智能制造技术研究院有限公司，湖南邵阳 422213；3.揭阳职业技术学院 机电工程系，广东揭阳 522051）

0 引言

随着工厂人力成本的增加，生产自动化成为了一个不可逆的趋势。在自动化装配生产线上，取料、避障、放料等是非常常见的应用场景。在实现具有取料、放料，以及避障等动作的运动控制中，如果对成本没有要求的，一般采用机器人来实现［1－3］。采用机器人的解决方案，方案简单，操作方便，只需要按照固定的轨迹点进行示教，机器人内部会自动生成相应的运动轨迹，而且机器人具有自动平滑运动轨迹功能，所以机器人在避障点附近可以平滑地经过避障点，通过采用平滑功能，可以大大提升效率；但是由于机器人对安装的空间要求比较大，所以才有机器人方案，设备的体积会变大，另一方面，由于机器人在箱取料位或者放料位运动时，没有改变目标位置的功能，机器人只能按照既定的轨迹，固定的速度曲线来运动，所以需要在取料等待位或者放料等待位做减速运动并停止，然后才能去取料或者放料，这样不利于提升效率。如果对成本比较敏感，一般会采用三轴的机械手来实现，采用三轴机械手，由于在取料位和放料位分别有等待位，在这两个点有停顿，会有相应的加减速，从而影响效率［4－6］；由于存在避障点，所以三轴机械手不能做到三轴联动，导致采用三轴机械手的效率更慢［7－9］。采用本文技术方案，可以让三轴机械手高效地实现取料和放料的动作，能实现像机器人的运动轨迹平滑功能，和机器人的解决方案相比，该技术方案成本更低，对安装位置要求更低，实现方法更简单，设计的柔性更高。

1 三轴运动模组机械结构

三轴机械手如图1所示，图示的结构中包含了XYZ三个坐标轴，还有一个障碍物，一个取料位，既是轨迹点去取料位把标签吸附到轨迹点上，一个放料位，既是轨迹点把从取料位吸附的标签按照设定的轨迹运动到放料位，然后贴到放料位上。

图1 三轴机械手结构

取料时，需要达到一定的条件，料已经在取料点时，轨迹点才能去取料；同理，放料时，需要达到一定的条件，轨迹点才能允许去放料点，所以需要有一个取料等待位和放料等待位。同时由于有障碍物存在，还需要有一个避障点。由于取料点和放料点有一定的位置精度要求，所以去取料点和放料点的位置时，运动轨迹必须是直线；同时更好地取料和放料，离开取料点和放料点时，运动轨迹必须也要是直线。

本文设计的技术方案，不限于只有两个等待位或者一个避障点，对于多个等待位和多个避障点都适用。为了方便描述本文的技术方案，采用两个等待位和一个避障点的情况进行说明。

2 运动控制分析

2.1 三轴模组从取料位到放料位轨迹分析

如图2和图3所示，为轨迹点从取料位到放料位的轨迹，由于从取料点出发时，必须要走直线，所以从取料点开始运动后，只有X轴动作，Y轴和Z轴停止，当X轴运动到某一个能避开障碍物的地方，Y轴和Z轴开始动作，这种运动方式比等X轴到位后，Y轴和Z轴再开始运动，效率更高，运动时间更短；由于Y轴和Z轴开始动作是根据X轴运动到某一个固定的点来确定的，所以从取料点到放料等待点的轨迹是固定的，就能保证轨迹点不会碰到障碍物。当X轴到达最远距离后，Y轴和Z轴再运行一段时间后也会停止，当Y轴和Z轴到达放料等待位后，X轴再开始向放料等待位运动，X轴的运动过程分两种情况，在X轴往放料等待位运动过程中，如果在X轴开始减速前，放料的条件能够达到，允许放料后，利用伺服电机的目标位置更改并且立即生效的功能，在经过放料等待位时不需要减速，直接到达放料位置；第二种情况，当X轴开始减速后，放料的条件才达到，这种情况下，X轴在放料等待位减速到0后，且放料条件达到后，X轴再开始运动到放料位置。

图2 从取料点到放料点运动轨迹（a基于X－Z坐标）

图3 从取料点到放料点运动轨迹（b基于X－Y坐标）

2.2 去放料位Y－Z轴启动点分析

为了能准确确定Y轴和Z轴开始运动时，X轴的坐标值，从两个坐标系内分别计算出这个X轴坐标的最小值，然后从这两个最小值中选一个比较大的值，即是需要计算的值。首先需要找到避障点，即是运动轨迹只需要避开这个点，就能达到避障的目的，在X－Z坐标系内，找到障碍物中X轴的最大数值，同时也找到Z轴坐标最大的数据，这两个数值组成的坐标点就是避障点。

如图所示，障碍物的左上角即是要找的避障点，根据这个可以计算出在X－Z坐标系内的Y轴和Z轴开始动作X轴的坐标X1，X1≥a－v2×（T1＋2×（b1－b））／2v1，其中v2为X轴最高速度，T1为Z轴加速时间，v1为Z轴最高速度，而且假定X轴和Z轴的加速时间比运动时间要短很多；同理也可以计算出基于X－Y轴坐标系内Y轴和Z轴开始动作X轴的坐标X2，X2≥c－v2×（T1＋2×（d1－d））／2v1，其中v2为X轴最高速度，T1为Z轴加速时间，v1为Z轴最高速度。如果X1＞X2，X1就是需要的X轴坐标值；如果X1＜X2，X2就是需要的X坐标值。

2.3 从取料位到放料位速度分析

从取料点到放料点，Y轴和Z轴都只是从取料等待位到放料等待位，只有两个位置点，而且从放料点开始运动之后的一段时间内，以及轨迹点从放料等待位到放料运动过程中，这两个节点，Y轴和Z轴都必须是静止的，所以不论放料条件是否及时成立，Y轴和Z轴的速度曲线都是一样的，所以不讨论Y轴和Z轴的速度曲线，只讨论X轴速度曲线。由图4可以看到，当放料条件没有及时满足时，X轴会多一次加减速过程，这时速度曲线和传统的三轴模组控制方案的速度曲线是一致的，当放料条件及时满足时，X轴会减少一次加减速过程，这样就可以提升运动效率。

图4 X轴从取料点到放料点速度曲线

当放料条件能及时达到，轨迹点能平滑地从取料位运动到放料位，大大提升运动效率，减少运动节拍时间，同时还能减少运动机构加减速次数，减少运动机构由于加减速带来的磨损；且不会和障碍物干涉。

当放料条件没有能及时达到，轨迹点会在放料等待位进行等待，不会直接去放料等待位，利用伺服电机的目标位置连续更改且立即生效的功能［10－11］，可以实现两种情况都能达到使用要求。

2.4 三轴模组从放料位回取料位轨迹分析

如图5和图6所示，轨迹点从放料点返回取料点，由于从放料点出发时，必须要走直线，所以从放料点开始运动后，只有X轴动作，Y轴和Z轴停止，当X轴运动到某一个能避开障碍物的地方，Y轴和Z轴开始动作，这种运动方式，比等X轴到位后，Y轴和Z轴再开始运动，这样的效率更高，运动时间更短；由于Y轴和Z轴开始动作是根据X轴运动到某一个固定的点来确定的，所以从放料点到取料等待点的轨迹是固定的，就能保证轨迹点不会碰到障碍物。当X轴到达最远距离后，Y轴和Z轴再运动一段时间后也会停止，当Y轴和Z轴到达取料等待位后，X轴再开始向取料等待位运动，X轴的运动过程分两种情况，当X轴运动到取料等待位过程中，如果在X轴开始减速前，取料的条件能够达到，允许取料后，利用伺服电机的目标位置更改并且立即生效的功能，在经过取料等待位时不需要减速，直接到达取料位置；第二种情况，当X轴开始减速后，取料的条件才达到，这种情况下，X轴在取料等待位减速到0后，且取料条件达到后，X轴再开始运动到取料位置。

图5 从放料点到取料点运动轨迹（a基于X－Z坐标）

图6 从放料点到取料点运动轨迹（b基于X－Y坐标）

回取料位Y－Z轴启动点分析如下。

由于回取料位的轨迹和去放料位轨迹是一个类似的运动控制，根据2.2节分析原理，可以得到在X－Z坐标系内的Y轴和Z轴开始动作X轴的坐标X1，X1≥a－v2×（T1＋2×（b－b1））／2v1，其中v2为X轴最高速度，T1为Z轴加速时间，v1为Z轴最高速度，而且假定X轴和Z轴的加速时间比运动时间要短很多；同理也可以计算出基于X－Y轴坐标系内Y轴和Z轴开始动作X轴的坐标X2，X2≥c－v2×（T1＋2×（d－d1））／2v1，其中v2为X轴最高速度，T1为Z轴加速时间，v1为Z轴最高速度。X1和X2中比较大的那个值即为Z和Y轴开始启动X轴的坐标。

2.5 三轴模组从放料位回取料位速度分析

从放料点回到取料点的速度分析，与从取料点到放料点的情况类似，类似的分析并结合图7可以得到，当取料条件及时满足时，X轴会减少一次加减速过程，这样就可以提升运动效率。

图7 X轴从取料点到放料点速度曲线

当取料条件能及时达到，轨迹点能平滑地从放料位运动到取料位，大大提升运动效率，减少运动节拍时间，同时还能减少运动机构加减速次数，减少运动机构由于加减速带来的磨损；且不会和障碍物干涉。

当取料条件没有能及时达到，轨迹点会在取料等待位进行等待，不会直接去取料等待位，利用伺服电机的目标位置连续更改且立即生效的功能，可以实现两种情况都能达到使用要求。

从取料位到放料位的轨迹和从放料位到取料位的轨迹不一样，取料条件有没有及时成立，但是从取料位到放料位的轨迹都是一样的，都是固定的，只不过，当取料条件没有及时成立，轨迹点会在接近取料等待位进行减速，并减速到0，等取料条件成立后，再去取料。当取料条件及时成立，轨迹点不会在取料等待位进行减速，而是直接去取料。同理，当放料条件有没有及时成立，但是从放料位到取料位的轨迹都是一样的，当放料条件没有及时成立，轨迹点会在接近放料等待位进行减速，并减速到0，等放料条件成立后，再去放料。当放料条件及时成立，轨迹点不会再放料等待位进行减速，而是直接去放料。

3 应用效果分析

采用本文涉及的技术方案用到电表自动装铭牌的设备上，相比采用普通的三轴运动控制方法，可以使设备节拍从8 s减少到4.5 s，比传统的三轴运动模组控制方法效率提升了43%。

4 结束语

本文为具有取料、放料、避障等功能的运动控制场景，提供了一种全新的解决方案。采用三轴运动控制模组的机械结构，结合本文的运动控制方法，比传统的技术方案成本低，效率高。相比于多轴机械手，本文涉及的技术方案成本更低，更容易安装。相比于普通的三轴机械臂运动控制方案，本文设计的技术方案能大大提升效率，能大大减少节拍时间；同时还能实现三轴联动，且运动轨迹是固定不变的，保证轨迹点不会和障碍物干涉；面对取料和放料条件能否及时达到的多种情况时，都能按照固定的轨迹来达到设定的工艺要求；当取料条件或者时放料条件能及时达到，利用伺服电机中的设定位置实时更改，且立即生效的功能，来实现在取料等待点或者放料等待位不进行减速停止，而是高速通过等待位，直接去放料或者取料位，这样能大大提高效率，而且还能减少伺服电机的加减速次数来减少机械冲击。