CN115122340B - 一种步进式机械臂的闭环控制方法 - Google Patents

Abstract

本发明公开了一种步进式机械臂的闭环控制方法，属于小型工业机械臂领域。通过软硬件的结合对步进式机械臂中的常规步进电机进行技术改造升级，采用了实时监控负载电流，通过对步进电机的负载电流的实时精细管理，使步进式机械臂在小负载的时候避免消耗太多功率，从而克服现有技术中因负载太小，需要将同规格的步进式机械臂的负载提高一个数量级，如将500g的负载最大提高到5kg的问题；在受到不同负载的时候可以实时调整输出力矩，达到了步进电机在大负载的时候不会发生丢步现象；而且，通过人工拖动步进式机械臂末端按需要的工作运动轨迹动作，主控MCU通过闭环驱动器的反馈信息记录相应的动作轨迹；实现拖动轨迹示教的自动编程。

Description

一种步进式机械臂的闭环控制方法

技术领域

本发明涉及小型工业机械臂领域，更具体地说，涉及一种步进式机械臂的闭环控制方法。

背景技术

随着科技和人工智能的发展，灵活的多轴机械手在工业控制中应用非常的广泛，如锁螺丝机、点胶机、剥线机、贴标机、医疗检测仪、电子组装设备等需要灵活调整工艺动作的领域。这是因为多轴机械手不但可以很好的代替人工完成一些简单重复的动作，还可以应用在一些复杂的工序上，如上下料、物料抓取、物料移栽、自动装配这些都是现在多轴机械手可以完成工序。由于多轴机械手可以二十四小时持续工作，让工作节拍变得非常的短，可以满足工厂的高效高速的生产要求，多轴机械手的优势主要是体现在于大批量生产的专业企业中，如家电、家具、电子产品、化工等行业。

一般情况下，多轴机械手四轴以上的为大型机械手，如六轴机械手；四轴及以下的为小型机械手。现有的多轴机械手现实的应用中，无论大型机械手还是小型机械手，均存在各自的不足之处。对于大型机械手，如大型六轴机械手结构比较复杂，体积也相对比较大，生产制造成本较高，主要用于负载要求比较高的重型工业中，如汽车、冶金等行业。对于小型机械手，如四轴机械手结构相对比较简单，体积也相对比较小，生产制造成本较低，主要用于负载要求比较低的轻型工业中，如电子行业；小型机械手因为存在体积小，负载相对比较小（负载一般小于500G），以及工作范围过小的缺点，导致在工业上的应用面比较窄，如机械加工中应用就受到一定的制约。如果在某些负载较大应用要实现大规模的自动化制造的时，就必须要搭建整条生产流水线，小型机械手的这些性能根本就派不上用场，这时候就得采用大型机械手。而大型六轴机械手比较笨重且价格相对昂贵，对于绝大多数小型企业来说，购买几台大型六轴机械手在财务上负担不起。虽然大型机械手都满足这些条件，但是体型庞大以及特殊性，不仅会占用的工厂面积大，大材小用，而且维护成本高，开发还比较麻烦，非专业人士要想短时间内使用它还是比较困难的，阻碍企业的良性发展。

为了改善小型机械手的现有状况，申请号为“CN202010868307.9”的中国发明专利公开了一种步进电机驱动方法、步进电机驱动装置及小型机械臂，驱动板根据接收到的信号控制步进电机运动，其驱动板能够接收modbus信号和脉冲信号，驱动板既能以modbus命令的方式又能以脉冲命令的方式间隔地对步进电机进行控制；当驱动板接收modbus信号时，驱动板以modbus命令的方式对步进电机进行控制；当驱动板接收脉冲信号时，驱动板以脉冲命令的方式对步进电机进行控制；驱动板能够接收编码器发来的用于对步进电机进行闭环调节的闭环调节信号，并对步进电机进行闭环调节，该申请所提供的方案能够使得小型机械臂具有运动精度高、运动速度快，运动过程平稳顺畅且能够灵活控制运动轨迹的优点。但是，该申请存在如下一些缺陷，如对处于待机的步进电机或不是满负荷时的步进电机的电流控制不够精准，容易导致步进电机工作电流过大，导致步进电机的线圈发热较大，处于连续工作状态的步进电机整体温升过高，不利于节能环保和提高步进电机使用寿命；而且控制程序的编写程序需要用到较为专业的程序员才能编写，技术门槛高，不利于现场的人员现场调试编程。

发明内容

为解决现有步进式小型机械手存在的上述缺陷，本发明提供一种通过新增通讯协议对机械臂的各个关节实时访问并反馈当前位置实现精准运动控制、以及多个活动关节的步进电机实现同步操作的、负载增大、手拖着机械臂末端即可完成轨迹示教编程的步进式机械臂的闭环控制方法。

为实现上述目的，本发明采取的技术方案是：

一种步进式机械臂的闭环控制方法，所述步进式机械臂包括：作为主控的主控MCU、步进电机驱动板、闭环驱动器、3~6个控制步进式机械臂的轴关节动作的步进电机、以及可对主控MCU进行编程的手持编程器；主控MCU通过串口通讯模块与步进电机驱动板进行通讯连接，步进电机驱动板中的具有独立地址的各步进电机驱动模块与步进电机一一对应连接，步进电机通过闭环驱动器将步进电机的位置、电流大小、到位与否的实时状态通过通讯总线反馈回主控MCU；

所述步进电机驱动模块包括驱动步进电机动作的H桥驱动组件、驱动H桥驱动组件的前置驱动组件，主控MCU的PWM输出口通过前置驱动组件将控制脉冲信号传递到H桥驱动组件，从而驱动步进电机动作；

其中，H桥驱动组件包括4组H桥驱动单元，分别为A相H桥驱动单元、B相H桥驱动单元、C相H桥驱动单元、D相H桥驱动单元，每组H桥驱动单元驱动对应步进电机的四个绕组中的同相绕组，即A相绕组、B相绕组、C相绕组、D相绕组中的同相绕组；每组H桥驱动单元包括两个均由主控MCU通过前置驱动组件控制通断的功率晶体管，第一功率晶体管的输入端与驱动步进电机的电源连接，第二功率晶体管的输入端与第一功率晶体管的输出端串联连接，两个功率晶体管的串联连接处为驱动输出处，驱动输出处与对应相的绕组连接；4组H桥驱动单元设有H桥公共参考地，H桥公共参考地通过一个总线采样电阻接地；

其中，B相H桥驱动单元和D相H桥驱动单元中的第二功率晶体管的输出端均与H桥公共参考地连接，A相H桥驱动单元和C相H桥驱动单元中的第二功率晶体管的输出端均通过一个电流转换采样电阻与H桥公共参考地连接；电流转换采样电阻和总线采样电阻的两端均与各自对应的主控MCU上的AD转换接口连接；

所述闭环控制方法包括以下步进电机节能降耗增负荷的步骤：

步骤A1，获得电机相位输出的电压值，主控MCU通过电流转换采样电阻和总线采样电阻获得各采样电阻相应节点的电压值；并通过欧姆定理计算得各采样电阻相应节点的电流值；

步骤A2，获得空载电流，让步进式机械臂空载运行，主控MCU通过步骤A1获得步进电机空载时的空载电流，并记录；

步骤A3，获得负载电流，让步进式机械臂分别带动不同的负载运行，主控MCU通过步骤A1获得步进电机不同的负载时对应的负载电流，并记录；

步骤A4，获得负载电流和负载的线性关系，主控MCU通过步骤A2和步骤A3记录的空载电流和不同的负载时对应的负载电流，形成负载电流和负载的线性关系图表，并保存；

步骤A5，获得实际负载时采样电流，在工作状态下，主控MCU通过步骤A1获得步进电机的负载电流；

步骤A6，判断实际负载时采样电流能否驱动步进电机运转，主控MCU将步骤A5的负载电流与步骤A4保存的负载电流和负载的线性关系图表进行比较，并判断状态为空载或负载；若判断为空载，主控MCU则在相应的PWM输出口输出对应占空比PWM，使步进电机工作在最低电流的空载节能模式；若判断为负载，则步进电机进入负载节能模式工作；

步骤A7，调节PWM占空比增大功率晶体管的电流输出，在负载节能模式下，若判断为当前负载电流不足以驱动步进电机运转了，主控MCU则对相应的PWM输出口增大输出的PWM占空比，直到可以驱动该负载为止；从而达到了负载大小与步进电机电流的科学匹配，避免大电流驱动小负载的情况；实现了实时监控负载电流和调整负载电流；避免了无论什么情况都满载电流的情况发生，从而降低了步进电机的各相绕组的发热量；提高了步进电机的各相绕组的热阻性能和过载性能；

步骤A8，从步骤A5~步骤A7之间循环。

进一步地，所述功率晶体管为MOS功率晶体管；所述前置驱动组件包括4组结构相同的前置驱动单元，分别为A相前置驱动单元、B相前置驱动单元、C相前置驱动单元、D相前置驱动单元，每组前置驱动单元驱动H桥驱动组件中对应的同相H桥驱动单元，即A相H桥驱动单元、B相H桥驱动单元、C相H桥驱动单元、D相H桥驱动单元的同相H桥驱动单元；前置驱动单元包括型号为LKS560的集成MOS驱动芯片，该集成MOS驱动芯片的高侧输入端和低侧输入端分别与主控MCU中的各自相对应的PWM输出口连接；该集成MOS驱动芯片的高侧输出端与相应的H桥驱动单元中的第一功率晶体管的控制端连接；该集成MOS驱动芯片的低侧输出端与相应的H桥驱动单元中的第二功率晶体管的控制端连接。

进一步地，所述闭环控制方法还包括步进式机械臂完成一个工作的运动轨迹控制，该运动轨迹控制包括以下步骤：

步骤01，编程，在手持编程器上面设计步进式机械臂的运动轨迹程序；

步骤02，运行程序，在手持编程器上点击运行运动轨迹程序，主控MCU通过通讯总线向步进电机驱动板发送控制信号；

步骤03，轨迹运动，步进电机驱动板接收到控制信号后向相应的步进电机发送控制脉冲值，相应的步进电机依此进行运动；

步骤04，闭环控制反馈，相应的步进电机运动完之后，步进电机驱动板通过闭环驱动器将目标值反馈回主控MCU，主控MCU将反馈回目标值跟设定值进行对比，如果小于设定值则添加脉冲值，直到跟设定值吻合；如果大于设定值则减少脉冲值，直到跟设定值吻合；

步骤15，运动轨迹完成，各步进电机运动到位后，步进电机驱动板通过闭环驱动器将目标值反馈回主控MCU，返回一个到位信号，进行下一步操作，直到完成了整体运动轨迹。步进式机械臂通过这种方法可以完成点对点，点对直线，直线对直线，点对圆弧等轨迹，使小型机械臂快速地改变姿态。多变的轨迹配合可以用于处理不同场景的要求。

进一步地，所述手持编程器包括针对步进式机械臂动作轨迹进行自动编程的拖动轨迹示教模块，拖动轨迹示教模块的自动编程步骤如下：

步骤11，开始拖动，在拖动轨迹示教模块的拖动轨迹示教自动编程状态下，人工拖动步进式机械臂末端按需要的工作运动轨迹动作，主控MCU通过闭环驱动器的反馈信息记录人工拖动步进式机械臂的动作轨迹；

步骤12，录入拖动，拖动轨迹示教模块根据步进式机械臂运动轨迹的反馈信息，将各步进电机的动作顺序和参数录入存贮，并自动翻译成步进式机械臂的运动轨迹程序，完成拖动轨迹示教的自动编程；

步骤13，编程确认，拖动轨迹示教自动编程完成进行确认并存贮在主控MCU的存储单元中，即可供主控MCU自动执行。

进一步地，所述步进式机械臂还包括多轴同步反馈功能，其步骤如下：主控MCU通过闭环驱动器的反馈信息，通过专门的通讯协议对步进电机状态的实时访问，步进电机状态包括步进电机的位置、状态、电流大小、以及是否到位。

进一步地，所述步进电机驱动板设有设定各步进电机驱动模块对应不同地址的拨码开关。

进一步地，所述手持编程器上设有人机交互的触摸屏。

由于步进式机械臂系统结合闭环驱动器的闭环算法，因此，在步进式机械臂受到不同负载的时候可以实时调整输出力矩，达到了步进电机在大负载的时候不会发生丢步现象；由于采用了实时监控负载电流，通过对步进电机的负载电流的实时精细管理，使步进式机械臂在小负载的时候避免消耗太多功率，从而克服现有技术中小型步进式机械臂只能用于小负载方面的应用问题，将同规格的步进式机械臂的负载提高一个数量级，如将500g的负载最大提高到5kg。当步进式机械臂的步进电机数量为6个时，在其应用场景中最多可添加六个自由度，可用于小型步进式机械臂的多关节联动，不占空间，负载能力比现有技术的小型步进式机械臂大幅提高 ，大幅降低了各种成本。可以按照实际需求，步进式机械臂可以任意匹配六个以内的自由度，工作场地即不受限制，小体积的步进式机械臂即可实现大负载的转变。为了解决现有的小型机械臂编程难和调试难的问题，通过创新性的拖动轨迹示教自动编程功能，直接拖着机械臂动作即可记录轨迹实现自动编程，不仅编程简单，而且还省去了很多调试的时间，零门槛，使用非常方便，特别适合在一些特殊行业或者没有专业编程技术人员的单位，而且，对于专业应用编程人员而言，也大幅提高了编程、调试的工作效率。

另外，由于普通的步进式小型机械臂没有闭环控制，现有步进式小型机械臂的步进电机控制系统只是作为一个接收端，没法将步进电机的实际运行信息反馈给系统的问题，并且无法避免会出现丢步现象，导致精度丢失。本发明通过增加软硬件匹配的方法，得到的多关节步进式机械臂，实现了体积小负载大、运动精度高、运动速度快、运动过程平稳顺畅且能够灵活控制运动轨迹的低成本技术改进，大幅减少企业的设备升级、改造和更新的成本，降低了常备应用技术人员的技术要求。

由上可知，相对于现有技术，本发明具有如下的优点：结构科学，从软硬件两方面有机地结合来进行综合技术改造升级，构思新颖，可以通过通讯总线实时读写参数，实时发指令控制，位置反馈等，通过闭环驱动器和通讯总线，步进电机位置控制更加精准、速度更加平缓；步进电机的负载电流根据负载实时调节，减少步进电机因总是处于满负载状态的工作电流下工作而导致温升高而发烫问题，使能耗大幅减少，同时也使负载能力大幅增加。通过拖动轨迹示教自动编程，直接拖动步进式机械臂即可完成示教路径并依此自动编成步进式机械臂的控制程序，让编写程序变得简单方便。

附图说明

图1为本发明的步进式机械臂的原理框图。

图2为本发明的步进式机械臂的结构框图。

图3为H桥驱动组件的电路原理图。

图4为前置驱动组件的电路原理图。

图5为图3中H桥驱动组件的功率晶体管开断逻辑控制关系图。

图6为步进电机的各相绕组对应的工作时序图。

主要附图标记：步进式机械臂-10，主控MCU-11，手持编程器-12，步进电机驱动板-13，步进电机-14，闭环驱动器-15，串口通讯模块-16。

具体实施方式

下面结合附图和优选的实施方式，对本发明及其有益技术效果进行进一步详细说明。

参见图1~图3，本发明优选实施的一种步进式机械臂的闭环控制方法，所述步进式机械臂10包括：作为主控的主控MCU11、步进电机驱动板13、闭环驱动器15、3个控制步进式机械臂10的轴关节动作的步进电机14、以及可对主控MCU11进行编程的手持编程器12；主控MCU11通过串口通讯模块16与步进电机驱动板13进行通讯连接，步进电机驱动板13中的具有独立地址的各步进电机14驱动模块与步进电机14一一对应连接，步进电机14通过闭环驱动器15将步进电机14的位置、电流大小、到位与否的实时状态通过通讯总线反馈回主控MCU11；

参见图3，所述步进电机14驱动模块包括驱动步进电机14动作的H桥驱动组件、驱动H桥驱动组件的前置驱动组件，主控MCU11的PWM输出口通过前置驱动组件将控制脉冲信号传递到H桥驱动组件，从而驱动步进电机14动作；

其中，H桥驱动组件包括4组H桥驱动单元，分别为A相H桥驱动单元、B相H桥驱动单元、C相H桥驱动单元、D相H桥驱动单元，每组H桥驱动单元驱动对应步进电机14的四个绕组中的同相绕组，即A相绕组、B相绕组、C相绕组、D相绕组中的同相绕组；每组H桥驱动单元包括两个均由主控MCU11通过前置驱动组件控制通断的功率晶体管（Q1~Q8），第一功率晶体管的输入端与驱动步进电机14的电源连接，第二功率晶体管的输入端与第一功率晶体管的输出端串联连接（对应电路网络连接标记分别为AO、BO、CO、DO），两个功率晶体管的串联连接处为驱动输出处，驱动输出处与对应相的绕组连接；4组H桥驱动单元设有H桥公共参考地PGND，H桥公共参考地PGND通过一个总线采样电阻PR3接地；

其中，B相H桥驱动单元和D相H桥驱动单元中的第二功率晶体管的输出端均与H桥公共参考地PGND连接，A相H桥驱动单元和C相H桥驱动单元中的第二功率晶体管的输出端均通过一个电流转换采样电阻（PR1、PR32）与H桥公共参考地PGND连接；电流转换采样电阻（PR1、PR32）和总线采样电阻PR3的两端均与各自对应的主控MCU11上的AD转换接口连接；

所述闭环控制方法包括以下步进电机节能降耗增负荷的步骤：

步骤A1，获得电机相位输出的电压值，主控MCU11通过电流转换采样电阻（PR1、PR32）和总线采样电阻PR3获得各采样电阻相应节点的电压值；并通过欧姆定理计算得各采样电阻相应节点的电流值；

步骤A2，获得空载电流，让步进式机械臂10空载运行，主控MCU11通过步骤A1获得步进电机14空载时的空载电流，并记录；

步骤A3，获得负载电流，让步进式机械臂10分别带动不同的负载运行，主控MCU11通过步骤A1获得步进电机14不同的负载时对应的负载电流，并记录；

步骤A4，获得负载电流和负载的线性关系，主控MCU11通过步骤A2和步骤A3记录的空载电流和不同的负载时对应的负载电流，形成负载电流和负载的线性关系图表，并保存；

步骤A5，获得实际负载时采样电流，在工作状态下，主控MCU11通过步骤A1获得步进电机14的负载电流；

步骤A6，判断实际负载时采样电流能否驱动步进电机14运转，主控MCU11将步骤A5的负载电流与步骤A4保存的负载电流和负载的线性关系图表进行比较，并判断状态为空载或负载；若判断为空载，主控MCU11则在相应的PWM输出口输出对应占空比PWM，使步进电机14工作在最低电流的空载节能模式；若判断为负载，则步进电机进入负载节能模式工作；

步骤A7，调节PWM占空比增大功率晶体管的电流输出，在负载节能模式下，若判断为当前负载电流不足以驱动步进电机14运转了，主控MCU11则对相应的PWM输出口增大输出的PWM占空比，直到可以驱动该负载为止；转动速度跟节拍有关，而步进电机14的负载能力跟驱动电流的大小有关；从而达到了负载大小与步进电机14电流的科学匹配，避免大电流驱动小负载的情况；实现了实时监控负载电流和调整负载电流；避免了无论什么情况都满载电流的情况发生，从而降低了步进电机14的各相绕组的发热量；提高了步进电机14的各相绕组的热阻性能和过载性能；因此，控制了步进电机14的温升，也就提高了步进电机14的载荷能力，也即与现有技术中的步进式机械臂10在同样步进电机14的温升情况下，负载电流提高了，负载能力也就提高了；

步骤A8，从步骤A5~步骤A7之间循环。

需要注意的是，本实施利仅列举3个步进电机的情况下的3轴步进式机械臂，每个步进电机的控制方式都是一样的。对于超过3个步进电机的情况下的多轴步进式机械臂，不另作说明。

参见图3~图4，优选地，所述功率晶体管（Q1~Q8）为MOS功率晶体管；所述前置驱动组件包括4组结构相同的前置驱动单元，分别为A相前置驱动单元、B相前置驱动单元、C相前置驱动单元、D相前置驱动单元，每组前置驱动单元驱动H桥驱动组件中对应的同相H桥驱动单元，即A相H桥驱动单元、B相H桥驱动单元、C相H桥驱动单元、D相H桥驱动单元的同相H桥驱动单元；前置驱动单元包括型号为LKS560的集成MOS驱动芯片，该集成MOS驱动芯片的高侧输入端和低侧输入端分别与主控MCU11中的各自相对应的PWM输出口连接；该集成MOS驱动芯片的高侧输出端与相应的H桥驱动单元中的第一功率晶体管的控制端连接；该集成MOS驱动芯片的低侧输出端与相应的H桥驱动单元中的第二功率晶体管的控制端连接。

在图3和图4所示的电路原理图中，电路网络连接标记表示电气连接在一起。通过图2中的4组结构相同的前置驱动单元，参见图2所示，集成MOS驱动芯片（U1~U4）的高侧输出端（对应电路网络连接标记AHO、BHO、CHO、DHO）和低侧输出端（对应电路网络连接标记ALO、BLO、CLO、DLO）输出三角波的控制相应相位的MOS功率晶体管控制端电压，从而控制步进电机14的力矩。

图3中的集成MOS驱动芯片（U1~U4）的高侧输出端（对应电路网络连接标记AHO、BHO、CHO、DHO）和低侧输出端（对应电路网络连接标记ALO、BLO、CLO、DLO），分别匹配对应地连接在图4中的电路网络连接标记AHO、ALO、BHO、BLO、CHO、CLO、DHO、DLO上，也就是通过限流电阻（R1~R4、R9~R12）各自相对应的功率晶体管（Q1~Q8）的G极，以此控制功率晶体管（Q1~Q8）的通断状态，而步进电机14的四相绕组，分别接在各H桥驱动单元中的第二功率晶体管的输入端与第一功率晶体管的输出端串联连接处（对应电路网络连接标记分别为AO、BO、CO、DO），形成一个H桥，如图3中所示。

参见图3~图6，步进电机14的相位节拍控制原理如下：当要控制步进电机14转动的时候，主控MCU11发送四组差分信号的PWM波形给到集成MOS驱动芯片（U1~U4）的高侧输出端（对应电路网络连接标记AH、BH、CH、DH）和低侧输出端（对应电路网络连接标记AL、BL、CL、DL），由集成MOS驱动芯片（U1~U4）输出适合步进电机14的电压波形，从而开断功率晶体管（Q1~Q8）。参见图5~图6，用步进电机14A相绕组来举例说明，当步序为1时，A相绕组要获取到的是1，也就是高电平，其他相绕组为低电平0。此时控制A相绕组的MOS功率晶体管Q1导通，MOS功率晶体管Q5截止，A相H桥驱动单元中的第二功率晶体管的输入端与第一功率晶体管的输出端串联连接处A0输出高电平，其他相绕组对应的MOS功率晶体管Q2、MOS功率晶体管Q3、MOS功率晶体管Q4截止，MOS功率晶体管Q6、MOS功率晶体管Q7、MOS功率晶体管Q8导通，其他相对应的H桥驱动单元中的第二功率晶体管的输入端与第一功率晶体管的输出端串联连接处（BO、CO、DO）输出低电平，步进电机14往前一步。参见图5，当8个步序走完，步进电机14转动一圈。如果主控MCU11发送脉冲频率越频繁，步进电机14转动速度就越快。

为了让步进电机14的能耗降到最低，此时电流转换采样电阻（PR1、PR32）和总线采样电阻PR3的两端均已经有步进电机14各相位输出的电压值了，通过总线采样电阻PR3和电流转换采样电阻（PR1、PR32）组成分压器获得的电压值，再通过欧姆定理即可算出此时各采样电阻对应的电流大小，对步进电机14工作时的工作电流进行监测。进而实现电流和力矩的自动匹配功能，即主控MCU11通过对步进电机14工作时的电流监测，即可根据负载物体的重量大小调节不同的电流，在力矩的范围之内匹配适应的力矩；对处于待机状态的步进电机14，按待机状态进行低电流和停电处理。

优选地，所述闭环控制方法还包括步进式机械臂10完成一个工作的运动轨迹控制，该运动轨迹控制包括以下步骤：

步骤01，编程，在手持编程器12上面设计步进式机械臂10的运动轨迹程序；

步骤02，运行程序，在手持编程器12上点击运行运动轨迹程序，主控MCU11通过通讯总线向步进电机驱动板13发送控制信号；

步骤03，轨迹运动，步进电机驱动板13接收到控制信号后向相应的步进电机14发送控制脉冲值，相应的步进电机14依此进行运动；

步骤04，闭环控制反馈，相应的步进电机14运动完之后，步进电机驱动板13通过闭环驱动器15将目标值反馈回主控MCU11，主控MCU11将反馈回目标值跟设定值进行对比，作最后的位置调整，如果小于设定值则添加脉冲值，直到跟设定值吻合；如果大于设定值则减少脉冲值，直到跟设定值吻合；

步骤15，运动轨迹完成，各步进电机14运动到位后，步进电机驱动板13通过闭环驱动器15将目标值反馈回主控MCU11，返回一个到位信号，进行下一步操作，直到完成了整体运动轨迹。步进式机械臂10通过这种方法可以完成点对点，点对直线，直线对直线，点对圆弧等轨迹，使小型机械臂快速地改变姿态。多变的轨迹配合可以用于处理不同场景的要求。

参见图2，优选地，所述步进电机驱动板13设有设定各步进电机14驱动模块对应不同地址的拨码开关。

参见图1，优选地，所述手持编程器12上设有人机交互的触摸屏。

其中，闭环驱动器15集成位置环，电流环，速度环等闭环算法控制，让步进电机14位置更加精准，具有可防“丢步”特性；速度更加平缓，电流根据负载实时调节，减少步进电机14发烫问题，减少能耗，从而把负载重量加大。

实际使用时，主控MCU11的网线接口通过作为通讯总线的网线连接步进电机驱动板13的网线接口，步进电机14驱动连接好步进电机14，各步进电机14可通过步进电机驱动板13上的拨码开关分别对应不同网络地址，从而主控MCU11就可以在重新上电的时候识别每个步进电机14的编号。在上电之后，直接用手持编程器12上的触摸屏校正原点，即可对步进电机14进行闭环调节。

通过这种闭环控制方法，能够在不同环境下对步进电机14进行闭环操作。例如，通过手持编程器12即可设计械臂工作时的运动轨迹程序，运动轨迹程序编写完成后，点击运行时主控MCU11通过网线向步进电机驱动板13发送控制信号，驱动板接收到信号之后向相应的控制步进电机14发送脉冲值使之进行运动。控制步进电机14运动完之后，主控MCU11将目标值跟设定值进行对比，做最后的位置调整，如果小于设定值则添加脉冲值，直到跟设定值吻合。反之亦然。运动到位后返回到位信号，让主控MCU11进行下一步操作。由此，小型机械臂可以实现点对点、点对直线、直线对直线、点对圆弧等轨迹运动，使小型机械臂快速地改变姿态。小型机械臂多变的轨迹配合可以用于处理不同场景的需求。

优选地，所述手持编程器12包括针对步进式机械臂10动作轨迹进行自动编程的拖动轨迹示教模块，拖动轨迹示教模块的自动编程步骤如下：

步骤11，开始拖动，在拖动轨迹示教模块的拖动轨迹示教自动编程状态下，人工拖动步进式机械臂10末端按需要的工作运动轨迹动作，主控MCU11通过闭环驱动器15的反馈信息记录人工拖动步进式机械臂10的动作轨迹；

步骤12，录入拖动，拖动轨迹示教模块根据步进式机械臂10运动轨迹的反馈信息，将各步进电机14的动作顺序和参数录入存贮，并自动翻译成步进式机械臂10的运动轨迹程序，完成拖动轨迹示教的自动编程；各步进电机14的参数包括位置环、速度环各自相对应的位置坐标值、角度值、延时值，并根据这些参数进行计算各步进电机14相应动作时的脉冲值等参数；

步骤13，编程确认，拖动轨迹示教自动编程完成进行确认并存贮在主控MCU11的存储单元中，即可供主控MCU11自动执行，从而实现拖动示教编程功能。

步进式机械臂10的拖动轨迹示教编程功能，即拖动步进式机械臂10末端，即可完成编程，从而大大降低了人工研发成本以及提高了工作效率。由于直接拖动步进式机械臂即可完成示教路径，从面可方便地增加了S型运动算法控制，让步进电机14运动更加顺畅、平缓，并支持多个电机，实现多轴联动。

所述步进式机械臂10还包括多轴同步反馈功能，其步骤如下：主控MCU11通过闭环驱动器15的反馈信息，通过专门的通讯协议对步进电机14状态的实时访问，步进电机14状态包括步进电机14的位置、状态、电流大小、以及是否到位。

步进式机械臂10在不同负载的场景时，由于步进电机14的负载能力和其工作电流成正比，步进电机14的工作电流和力矩的自动匹配后，主控MCU11会根据负载物体大小调节不同的电流驱动相应的步进电机14，从而在步进电机14的力矩范围之内科学地匹配适当的力矩，产生的技术效果就是当步进式机处于待机的状态下（没有负载时），工作电流几乎为零，步进式机的功率损耗就可以降到最低水平，从而从根源上解决步进电机14因工作电流保持在高水平状态而导致步进电机14线圈温升高，进而导致步进电机14一直发烫的问题。当步进式机械臂10的负载为3~5kg的时候，电流匹配的力矩效率最大，更好的发挥步进电机14的性能。即便机械臂做得很小，也不会影响负载。从而实现了步进式机械臂10小体积大负载的转变，对于一些工作区域空间比较狭小的地方，而对负载要求比较大的工作岗位，从硬件成本、运行成本、调试成本、人工成本和安装空间方面可以得到很好的解决方案。

由于步进式机械臂10是通过轴和关节来实现动作变化的，轴和关节是通过步进式机来驱动的。因此，本发明的步进式机械臂10具有多轴同步反馈功能，多轴同步的实用性在于可以很精确地到达某一个指定点，并且轨迹设置简单。如果按逐个单个关节示教的话会面临比较多的问题，第一，运动轨迹不定；第二，示教时很难定点，因为单个轴去配合的话，只能点对点，也就是每次只能控制一个轴，串行式的控制方式，导致控制量较大，各轴互不关联动作，彼此不配合，只能是各走各的，就会出现很难去精确对准目标，造成浪费大量时间，出现很多盲区，很小的控制量也要调试很久才能到位，导致耗费大量的时间，工作效率低。而且，现有的普通步进式机械臂10并没有到位反馈功能，从而导致主控MCU11无法确认步进式机械臂10什么时候到达目标，如果步进式机械臂10没有及时到位就开始下一个动作就会导致步进式机械臂10出错，从而降低了准确率。而多轴联动起来，控制间隙小，操控性强，多个关节同时向着一点出发，同时到达一个位置。不仅步距间可以微调，还固定了运动轨迹，防止发生冲撞问题，步进式机械臂10在到位后通过通讯协议发送到位信号给主控MCU11。主控MCU11接到信号后才开始执行下一步操作，避免了步骤出错，也能及时得到步进式机械臂10的精准位置。这样调试起来省掉了大部分的运行时间，还确保了不会出现运行轨迹的不确定性的问题。

本发明通过通讯协议对步进电机14现在的位置、状态、电流大小、以及到位与否等可对步进电机14状态的实时访问。从而获得步进式机械臂开始丢步的时间节点和丢步原因，开发人员排查问题更为方便快捷，节省排查问题的时间。

手持编程器12把指令发送给主控MCU11，然后主控MCU11同时把信号分发给步进电机驱动板13中的各个步进电机14驱动模块，使多个步进电机14协调运动，使得闭环式械臂具有体积小负载大、运动精度高、运动速度快，运动过程平稳顺畅且能够灵活控制运动轨迹的优点。

本发明主要解决了现有步进式小型机械手存在的负载小、缺少工作时状态信息反馈、故障率高、编程和调试不方便的问题。本发明的步进式机械臂10相应的按实际应用需求安装上工装夹具之后，就可以作小型机械手使用，方便一些轻工业企业实现低成本的应用方案。

上述说明内容中，本发明属于控制方法的改进，属于软件方法类的创新。步进式机械臂10所涉及硬件，如机械构造、电子电路等未在本说明书提示的内容，均属于现有技术，其具体型号、参数，可根据实际使用情况互相匹配调整，属于现有技术中常用的技术，对属于现有技术中常规使用的内容，如结构和工艺，为了节省篇幅，不再赘述。未揭示的加工工艺和零件、元器件，按现有技术的常规技术处理即可。

根据上述说明书及具体实施例并不对本发明构成任何限制，本发明并不局限于上面揭示和描述的具体实施方式，对本发明的控制思路及方法以不同的方式实施或转变，也应当落入本发明的权利要求的保护范围内。

Claims (4)

1.一种步进式机械臂的闭环控制方法，所述步进式机械臂包括：作为主控的主控MCU、步进电机驱动板、闭环驱动器、3~6个控制步进式机械臂的轴关节动作的步进电机、以及可对主控MCU进行编程的手持编程器；主控MCU通过串口通讯模块与步进电机驱动板进行通讯连接，步进电机驱动板中的具有独立地址的各步进电机驱动模块与步进电机一一对应连接，步进电机通过闭环驱动器将步进电机的位置、电流大小、到位与否的实时状态通过通讯总线反馈回主控MCU；

其特征是，所述步进电机驱动模块包括驱动步进电机动作的H桥驱动组件、驱动H桥驱动组件的前置驱动组件，主控MCU的PWM输出口通过前置驱动组件将控制脉冲信号传递到H桥驱动组件，从而驱动步进电机动作；

其中，H桥驱动组件包括4组H桥驱动单元，分别为A相H桥驱动单元、B相H桥驱动单元、C相H桥驱动单元、D相H桥驱动单元，每组H桥驱动单元驱动对应步进电机的四个绕组中的同相绕组，即A相绕组、B相绕组、C相绕组、D相绕组中的同相绕组；每组H桥驱动单元包括两个均由主控MCU通过前置驱动组件控制通断的功率晶体管，第一功率晶体管的输入端与驱动步进电机的电源连接，第二功率晶体管的输入端与第一功率晶体管的输出端串联连接，两个功率晶体管的串联连接处为驱动输出处，驱动输出处与对应相的绕组连接；4组H桥驱动单元设有H桥公共参考地，H桥公共参考地通过一个总线采样电阻接地；

其中，B相H桥驱动单元和D相H桥驱动单元中的第二功率晶体管的输出端均与H桥公共参考地连接，A相H桥驱动单元和C相H桥驱动单元中的第二功率晶体管的输出端均通过一个电流转换采样电阻与H桥公共参考地连接；电流转换采样电阻和总线采样电阻的两端均与各自对应的主控MCU上的AD转换接口连接；

所述闭环控制方法包括以下步进电机节能降耗增负荷的步骤：

步骤A1，获得电机相位输出的电压值，主控MCU通过电流转换采样电阻和总线采样电阻获得各采样电阻相应节点的电压值；并通过欧姆定理计算得各采样电阻相应节点的电流值；

步骤A2，获得空载电流，让步进式机械臂空载运行，主控MCU通过步骤A1获得步进电机空载时的空载电流，并记录；

步骤A3，获得负载电流，让步进式机械臂分别带动不同的负载运行，主控MCU通过步骤A1获得步进电机不同的负载时对应的负载电流，并记录；

步骤A4，获得负载电流和负载的线性关系，主控MCU通过步骤A2和步骤A3记录的空载电流和不同的负载时对应的负载电流，形成负载电流和负载的线性关系图表，并保存；

步骤A5，获得实际负载时采样电流，在工作状态下，主控MCU通过步骤A1获得步进电机的负载电流；

步骤A6，判断实际负载时采样电流能否驱动步进电机运转，主控MCU将步骤A5的负载电流与步骤A4保存的负载电流和负载的线性关系图表进行比较，并判断状态为空载或负载；若判断为空载，主控MCU则在相应的PWM输出口输出对应占空比PWM，使步进电机工作在最低电流的空载节能模式；若判断为负载，则步进电机进入负载节能模式工作；

步骤A7，调节PWM占空比增大功率晶体管的电流输出，在负载节能模式下，若判断为当前负载电流不足以驱动步进电机运转了，主控MCU则对相应的PWM输出口增大输出的PWM占空比，直到可以驱动该负载为止；

步骤A8，从步骤A5~步骤A7之间循环。

2.如权利要求1所述的步进式机械臂的闭环控制方法，其特征是：所述功率晶体管为MOS功率晶体管；所述前置驱动组件包括4组结构相同的前置驱动单元，分别为A相前置驱动单元、B相前置驱动单元、C相前置驱动单元、D相前置驱动单元，每组前置驱动单元驱动H桥驱动组件中对应的同相H桥驱动单元，即A相H桥驱动单元、B相H桥驱动单元、C相H桥驱动单元、D相H桥驱动单元的同相H桥驱动单元；前置驱动单元包括型号为LKS560的集成MOS驱动芯片，该集成MOS驱动芯片的高侧输入端和低侧输入端分别与主控MCU中的各自相对应的PWM输出口连接；该集成MOS驱动芯片的高侧输出端与相应的H桥驱动单元中的第一功率晶体管的控制端连接；该集成MOS驱动芯片的低侧输出端与相应的H桥驱动单元中的第二功率晶体管的控制端连接。

3.如权利要求1或2所述的步进式机械臂的闭环控制方法，其特征是：所述闭环控制方法还包括步进式机械臂完成一个工作的运动轨迹控制，该运动轨迹控制包括以下步骤：

步骤01，编程，在手持编程器上面设计步进式机械臂的运动轨迹程序；

步骤02，运行程序，在手持编程器上点击运行运动轨迹程序，主控MCU通过通讯总线向步进电机驱动板发送控制信号；

步骤03，轨迹运动，步进电机驱动板接收到控制信号后向相应的步进电机发送控制脉冲值，相应的步进电机依此进行运动；

步骤04，闭环控制反馈，相应的步进电机运动完之后，步进电机驱动板通过闭环驱动器将目标值反馈回主控MCU，主控MCU将反馈回目标值跟设定值进行对比，如果小于设定值则添加脉冲值，直到跟设定值吻合；如果大于设定值则减少脉冲值，直到跟设定值吻合；

步骤15，运动轨迹完成，各步进电机运动到位后，步进电机驱动板通过闭环驱动器将目标值反馈回主控MCU，返回一个到位信号，进行下一步操作，直到完成了整体运动轨迹。

4.如权利要求3所述的步进式机械臂的闭环控制方法，其特征是：所述手持编程器包括针对步进式机械臂动作轨迹进行自动编程的拖动轨迹示教模块，拖动轨迹示教模块的自动编程步骤如下：

步骤11，开始拖动，在拖动轨迹示教模块的拖动轨迹示教自动编程状态下，人工拖动步进式机械臂末端按需要的工作运动轨迹动作，主控MCU通过闭环驱动器的反馈信息记录人工拖动步进式机械臂的动作轨迹；

步骤12，录入拖动，拖动轨迹示教模块根据步进式机械臂运动轨迹的反馈信息，将各步进电机的动作顺序和参数录入存贮，并自动翻译成步进式机械臂的运动轨迹程序，完成拖动轨迹示教的自动编程；

步骤13，编程确认，拖动轨迹示教自动编程完成进行确认并存贮在主控MCU的存储单元中，即可供主控MCU自动执行。

Images