(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210628323.X (22)申请日 2022.06.06 (71)申请人 宁波安信数控技 术有限公司 地址 315801 浙江省宁波市北仑区小港小 浃江中路518号 (72)发明人 汤海天　胡天恩　张奇之　顾晓栋　 李航　 (74)专利代理 机构 宁波奥圣专利代理有限公司 33226 专利代理师 周珏 (51)Int.Cl. B25J 19/00(2006.01) B25J 9/16(2006.01) (54)发明名称 一种注塑机 机械手碰撞检测方法 (57)摘要 本发明公开了一种注塑机机械手碰撞检测 方法， 其利用采集模块采集各轴的伺服电机的数 据并对数据进行滤波， 在全自动运行第一模时利 用标定模块根据滤波后的数据判定各轴的运动 状态并标定该运动状态下的最大扭矩值和最小 扭矩值， 利用标定模块同时标定各轴的反馈速度 和指令速度之间的速度时延， 在运行后续每模时 利用碰撞检测模块根据各轴的指令速度和速度 时延预测实际速度和实际加速度判定各轴的运 动状态， 并根据各轴的运动状态、 该运动状态下 的最大扭矩 值和最小 扭矩值、 检测灵敏度以及滤 波后的数据识别各轴是否发生碰撞； 优点是能在 不改变机械手结构以及不增加机械手制造成本 的情况下实现机械手与外部环 境的碰撞检测， 且 检测精度高。 权利要求书5页 说明书11页 附图3页 CN 115091503 A 2022.09.23 CN 115091503 A 1.一种注塑机 机械手碰撞检测方法， 其特 征在于包括以下步骤： 步骤1： 在机 械手控制器中设置采集模块、 标定模块、 碰撞检测模块； 步骤2： 机械手全自动运行第一模的过程中， 在每个周期内采集模块和标定模块工作， 针对机械手的走行轴、 左右轴和上下轴， 采集模块采集各轴的伺服电机的数据并对数据进 行滤波处理， 标定模块根据滤波后的数据判定各轴的运动状态并标定该运动状态下的最大 扭矩值和最小扭矩值， 针对当前周期的具体过程 为： 步骤2_1： 将在当前周期内采集模块从各轴的伺服驱动器中读取的对应的伺服电机的 反馈扭矩和反馈 速度对应记为tfb和vfb； 步骤2_2： 分别对tfb和vfb进行滤波处理， 对应 得到滤波处理后的滤波反馈 扭矩和滤波反 馈速度， 对应记为tf和vf； 步骤2_3： 在当前周期内标定模块根据 vf、 v′f和T， 计算在当前周期内各轴的伺服电机的 加速度， 记为af， 其中， 若当前周期为机械手全自动运行第一模的第一个周期， 则在 中令v′f＝vf， 即v′f的初始值为vf； 若当前周期不为机械手全自动运行第一 模的第一个周期， 则v ′f表示各轴的伺服电机在上一周期的反馈速度经滤波处理后得到 的 滤波反馈 速度， T表示 一个周期的时长； 步骤2_4： 在当前周期内标定模块根据 vf和af， 判定在当前周期内各轴的运动状态， 具体 为： 当vf＞0且af＞0时， 判定在当前周期内对应轴处于正向加速状态； 当vf＞0且af＝0时， 判 定在当前周期内对 应轴处于正向匀速状态； 当vf＞0且af＜0时， 判定在当前周期内对 应轴处 于正向减速状态； 当vf＝0且af＝0时， 判定在当前周期内对应轴处于静止状态； 当vf＜0且af ＜0时， 判定在当前周期内对 应轴处于负向加速状态； 当vf＜0且af＝0时， 判定在当前周期内 对应轴处于负向匀速状态； 当vf＜0且af＞0时， 判定在当前周期内对应轴处于负向减速状 态； 步骤2_5： 在当前周期内标定模块根据在 当前周期内各轴的运动状态， 标定该运动状态 下的最大 扭矩值和最小扭矩值， 具体为： 在当前周期内对应轴处于正向加速状态时， 标定正向加速状态下的最大扭矩值和最小 扭矩值， 记为tpamax和tpamin， 若当前周期为机械手全自动运行第一模过程中第一次正向加速 状态下扭矩标定， 则令tpamax＝tf、 tpamin＝tf， 若当前周期不为机械手全自动运行第一模 过程 中第一次正向加速状态下扭矩标定， 则如果tf＞t'pamax， 那么更新tpamax的值， 令tpamax＝tf， 保 持tpamin的值不变， 即令tpamin＝t'pamin； 如果tf＜t'pamin， 那么更新tpamin的值， 令tpamin＝tf， 保 持tpamax的值不变， 即令tpamax＝t'pamax； 如果t'pamin≤tf≤t'pamax， 那么保持tpamax和tpamin的值 不变， 即令tpamax＝t'pamax、 tpamin＝t'pamin； 其中， t'pamax表示上一次标定的正向加速状态下的 最大扭矩值， t'pamin表示上一次标定的正向加速状态下的最小扭矩值； 在当前周期内对应轴处于正向匀速状态时， 标定正向匀速状态下的最大扭矩值和最小 扭矩值， 记为tpcmax和tpcmin， 若当前周期为机械手全自动运行第一模过程中第一次正向匀速 状态下扭矩标定， 则令tpcmax＝tf、 tpcmin＝tf， 若当前周期不为机械手全自动运行第一模 过程 中第一次正向匀速状态下扭矩标定， 则如果tf＞t'pcmax， 那么更新tpcmax的值， 令tpcmax＝tf， 保 持tpcmin的值不变， 即令tpcmin＝t'pcmin； 如果tf＜t'pcmin， 那么更新tpcmin的值， 令tpcmin＝tf， 保权　利　要　求　书 1/5 页 2 CN 115091503 A 2持tpcmax的值不变， 即令tpcmax＝t'pcmax； 如果t'pcmin≤tf≤t'pcmax， 那么保持tpcmax和tpcmin的值 不变， 即令tpcmax＝t'pcmax、 tpcmin＝t'pcmin； 其中， t'pcmax表示上一次标定的正向匀速状态下的 最大扭矩值， t'pcmin表示上一次标定的正向匀速状态下的最小扭矩值； 在当前周期内对应轴处于正向减速状态时， 标定正向减速状态下的最大扭矩值和最小 扭矩值， 记为tpdmax和tpdmin， 若当前周期为机械手全自动运行第一模过程中第一次正向减速 状态下扭矩标定， 则令tpdmax＝tf、 tpdmin＝tf， 若当前周期不为机械手全自动运行第一模 过程 中第一次正向减 速状态下扭矩标定， 则如果tf＞t'pdmax， 那么更新tpdmax的值， 令tpdmax＝tf， 保 持tpdmin的值不变， 即令tpdmin＝t'pdmin； 如果tf＜t'pdmin， 那么更新tpdmin的值， 令tpdmin＝tf， 保 持tpdmax的值不变， 即令tpdmax＝t'pdmax； 如果t'pdmin≤tf≤t'pdmax， 那么保持tpdmax和tpdmin的值 不变， 即令tpdmax＝t'pdmax、 tpdmin＝t'pdmin； 其中， t'pdmax表示上一次标定的正向减速状态下的 最大扭矩值， t'pdmin表示上一次标定的正向减速状态下的最小扭矩值； 在当前周期内对应轴处于静止状态时， 标定静止状态下的最大扭矩值和最小扭矩值， 记为tsmax和tsmin， 若当前周期为机械手全自动运行第 一模过程中第 一次静止状态下扭矩标 定， 则令tsmax＝tf、 tsmin＝tf， 若当前周期不为机械手全自动运行第一模过程中第一次静止 状态下扭矩标定， 则如果tf＞t'smax， 那么更新tsmax的值， 令tsmax＝tf， 保持tsmin的值不变， 即 令tsmin＝t'smin； 如果tf＜t'smin， 那么更新tsmin的值， 令tsmin＝tf， 保持tsmax的值不变， 即令 tsmax＝t'smax； 如果t'smin≤tf≤t'smax， 那么保持tsmax和tsmin的值不变， 即令tsmax＝t'smax、 tsmin ＝t'smin； 其中， t'smax表示上一次标定的静止状态下的最大扭矩值， t'smin表示上一次标定的 静止状态下的最小扭矩值； 在当前周期内对应轴处于负向加速状态时， 标定负向加速状态下的最大扭矩值和最小 扭矩值， 记为tnamax和tnamin， 若当前周期为机械手全自动运行第一模过程中第一次负向加速 状态下扭矩标定， 则令tnamax＝tf、 tnamin＝tf， 若当前周期不为机械手全自动运行第一模 过程 中第一次负向加速状态下扭矩标定， 则如果tf＞t'namax， 那么更新tnamax的值， 令tnamax＝tf， 保 持tnamin的值不变， 即令tnamin＝t'namin； 如果tf＜t'namin， 那么更新tnamin的值， 令tnamin＝tf， 保 持tnamax的值不变， 即令tnamax＝t'namax； 如果t'namin≤tf≤t'namax， 那么保持tnamax和tnamin的值 不变， 即令tnamax＝t'namax、 tnamin＝t'namin； 其中， t'namax表示上一次标定的负向加速状态下的 最大扭矩值， t'namin表示上一次标定的负向加速状态下的最小扭矩值； 在当前周期内对应轴处于负向匀速状态时， 标定负向匀速状态下的最大扭矩值和最小 扭矩值， 记为tncmax和tncmin， 若当前周期为机械手全自动运行第一模过程中第一次负向匀速 状态下扭矩标定， 则令tncmax＝tf、 tncmin＝tf， 若当前周期不为机械手全自动运行第一模 过程 中第一次负