(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202211038173.3 (22)申请日 2022.08.29 (65)同一申请的已公布的文献号 申请公布号 CN 115122340 A (43)申请公布日 2022.09.30 (73)专利权人 佛山市指擎科技有限公司 地址 528000 广东省佛山市南海区桃园东 路88号慧泉科技产业中心二期一、 二、 三栋研发车间的第二栋第三层F3A05- 2 (72)发明人 刘一流　卢佳酉　彭鼎　 (74)专利代理 机构 佛山中贵知识产权代理事务 所(普通合伙) 44491 专利代理师 王彦昌　李志良(51)Int.Cl. B25J 9/16(2006.01) B25J 13/06(2006.01) 审查员 李祥亮 (54)发明名称 一种步进式机 械臂的闭环控制方法 (57)摘要 本发明公开了一种步进式机械臂的闭环控 制方法， 属于小型工业机械臂领域。 通过软硬件 的结合对步进式机械臂中的常规步进电机进行 技术改造升级， 采用了实时监控负载电流， 通过 对步进电机的负载电流的实时精细管理， 使步进 式机械臂在小负载的时候避免消耗太多功率， 从 而克服现有技术中因负载太小， 需要将同规格的 步进式机械臂的负载提高一个数量级， 如将500g 的负载最大提高到5kg的问题； 在受到不同负载 的时候可以实时调整输出力矩， 达到了步进电机 在大负载的时候不会发生丢步现象； 而且， 通过 人工拖动步进式机械臂末端按需要的工作运动 轨迹动作， 主控MCU通过闭环驱动器的反馈信息 记录相应的动作轨迹； 实现拖动轨迹示教的自动 编程。 权利要求书2页 说明书10页 附图3页 CN 115122340 B 2022.11.18 CN 115122340 B 1.一种步进式机械臂的闭环控制方法， 所述步进式机械臂包括： 作为主控的主控MCU、 步进电机驱动板、 闭环驱动器、 3~6个控制步进式机械臂的轴关节动作的步进电机、 以及可 对主控MCU进 行编程的手持编程器； 主控MCU通过串口通讯模块与步进电机驱动板进 行通讯 连接， 步进电机驱动板中的具有独立地址的各步进电机驱动模块与步进电机一一对应连 接， 步进电机通过闭环驱动器将步进电机的位置、 电流大小、 到位与否的实时状态通过通讯 总线反馈回主控M CU； 其特征是， 所述步进电机驱动模块包括驱动步进电机动作的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桥公共参考地连接； 电流转换采样电阻和总线采样电阻的两端均与 各自对应的主控M CU上的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占 空比， 直到可以驱动该负载为止；权　利　要　求　书 1/2 页 2 CN 115122340 B 2步骤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桥驱动单元中的第一功率晶 体管的控制端连接； 该集成M OS驱动芯片的低侧输出端与相应的H桥驱动单元中的第二功 率 晶体管的控制端连接 。 3.如权利要求1或2所述的步进式机械臂 的闭环控制方法， 其特征是： 所述闭环控制方 法还包括步进式机 械臂完成一个工作的运动轨 迹控制， 该运动轨 迹控制包括以下步骤： 步骤01， 编程， 在手持编程器上面设计步进式机 械臂的运动轨 迹程序； 步骤02， 运行程序， 在手持编程器上点击运行运动轨迹程序， 主控MCU通过通讯总线向 步进电机驱动板发送控制信号； 步骤03， 轨迹运动， 步进电机驱动板接收到控制信号后向相应的步进电机发送控制脉 冲值， 相应的步进电机依此进行运动； 步骤04， 闭环控制反馈， 相应的步进电机运动完之后， 步进电机驱动板通过闭环驱动 器 将目标值反馈回主控MCU， 主控MCU将反馈回目标值跟设定值进行对比， 如果小于设定值则 添加脉冲值， 直到跟设定值吻合； 如果大于设定值则减少脉冲值， 直到跟设定值吻合； 步骤15， 运动轨迹完成， 各步进电机运动到位后， 步进电机驱动板通过闭环驱动 器将目 标值反馈回主控M CU， 返回一个到位信号， 进行 下一步操作， 直到 完成了整体运动轨 迹。 4.如权利要求3所述的步进式机械臂的闭环控制方法， 其特征是： 所述手持编程器包括 针对步进式机械臂动作轨迹进 行自动编程的拖动轨迹示教模块， 拖动轨迹示教模块的自动 编程步骤如下： 步骤11， 开始拖动， 在拖动轨迹示教模块的拖动轨迹示教 自动编程状态下， 人工拖动步 进式机械臂末端按需要的工作运动轨迹动作， 主控MCU通过闭环驱动器的反馈信息记录人 工拖动步进式机 械臂的动作轨 迹； 步骤12， 录入拖动， 拖动轨迹示教模块根据步进式机械臂运动轨迹的反馈信息， 将各步 进电机的动作顺序和 参数录入存贮， 并自动翻译成步进式机械臂的运动轨迹程序， 完成拖 动轨迹示教的自动 编程； 步骤13， 编程确认， 拖动轨迹示教 自动编程完成进行确认并存贮在主控MCU的存储单元 中， 即可供主控MCU自动执 行。权　利　要　求　书 2/2 页 3 CN 115122340 B 3