(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210884772.0 (22)申请日 2022.07.26 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市下城区潮王路 18号 (72)发明人 董辉　徐嘉晨　孙动动　罗立锋　 张成祥　俞立　 (74)专利代理 机构 杭州求是专利事务所有限公 司 33200 专利代理师 忻明年 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种异形空间内的机械手安全轨迹规划方 法 (57)摘要 本发明公开了一种异形空间内的机械手安 全轨迹规划方法， 包括： 异形空间示教， 示教异形 空间由两个矩形安全区构成， 并记录每个矩形安 全区对角两个点的坐标； 根据每个矩形安全区对 角两个点的坐标得到模内安全区范围； 轨迹的起 点与终点示教； 根据示教的起点、 交点和终点得 到由起点到交点之间的直线以及交点到终点之 间的圆弧构成的轨迹， 机械手运动时由起点直线 运动至交点， 并在交点与终点之间进行数字积分 圆弧插补直至运动至终点取物。 本发 明可保证机 械手在模内区域时安全运行防止撞击注塑机及 模具， 也可进一步地缩短机械手做取出动作的时 间， 与注塑机 配合， 提高注塑机的工作效率。 权利要求书2页 说明书7页 附图5页 CN 115401687 A 2022.11.29 CN 115401687 A 1.一种异形空间内的机械手安全轨迹规划方法， 用于注塑机机械手模内运动规划， 其 特征在于， 所述异形空间内的机 械手安全轨 迹规划方法， 包括： 步骤1、 异形空间示教， 示教异形空间由两个矩形安全区构成， 并记录每个矩形安全区 对角两个点的坐标； 步骤2、 根据每个矩形安全区对角两个点的坐标得到模内安全区范围， 所述模内安全区 范围包括Z轴模内安全区范围、 X轴模内安全区的范围和Y轴上 行安全区范围； 步骤3、 轨 迹的起点与终点 示教， 包括： 步骤3.1、 取每个矩形安全区对角两个点的坐标， 第 一矩形安全区对角两个点的坐标分 别为P1(x1， y1， z1)和P2(x2， y2， z2)， 第二矩形安全区对角两个点的坐标分别为P3(x3， y3， z3)和 P4(x4， y4， z4)， 其中， P1和P3为动模板上的位置， P2和P4为定模板上的位置； 步骤3.2、 设定轨迹 的起点为线段y＝y1， z1≤z≤z2上的任意一点， 轨迹 的终点为线段z ＝z3， y3≤y≤y4上的任意一点， 根据模内取物时物体位置示教终点Q坐标为(n， z3)， y3≤n≤ y4； 步骤3.3、 以坐标点P3为圆心、 n ‑y3为半径作圆， 获取圆与直线y＝ y2的交点T的坐标为(z3 +n‑y3， y2)， 并确定起 点P坐标为(z3+n‑y3， y1)； 步骤4、 轨迹运动规划： 根据步骤3示教的起点P、 交点T和终点Q得到由起点P到交点T之 间的直线以及交点T到终点Q之间的圆弧构成的轨迹， 机械手运动时由P点直线运动至T点， 并在T点与Q 点之间进行 数字积分圆弧插补运动至Q 点取物。 2.如权利要求1所述的异形空间内的机械手安全轨迹规划方法， 其特征在于， 所述异形 空间示教， 包括： 步骤1.1、 由机 械手原点移动Z轴至动模板位置， 记录该位置坐标为P1(x1， y1， z1)； 步骤1.2、 由位置P1移动Z轴至定模板位置， 然后移动Y轴向下至极限安全距离， 记录位置 坐标为P2(x2， y2， z2)， 完成第一矩形安全区示教； 步骤1.3、 由位置P2移动Z轴至动模板位置， 记录该位置坐标为P3(x3， y3， z3)； 步骤1.4、 由位置P3移动Z轴至定模板位置， 然后移动Y轴向下至极限安全距离， 记录位置 坐标为P4(x4， y4， z4)， 完成第二矩形安全区示教。 3.如权利要求2所述的异形空间内的机械手安全轨迹规划方法， 其特征在于， 所述根据 每个矩形安全区对角两个点的坐标 得到模内安全区范围， 包括： Z轴模内安全区范围为(z1， z4)， X轴模内安全区的范围为(x1， x2)， Y轴上行安全区范围为 (0， y1)， 第一矩形安全区的长为z2‑z1， 高为y2‑y1， 第二矩形安全区的长为z4‑z3， 高为y4‑y3。 4.如权利要求3所述的异形空间内的机械手安全轨迹规划方法， 其特征在于， 所述交点 T的坐标中Z轴坐标z3+n‑y3若满足z1≤z3+n‑y3≤z4， 则正常执行； 若不满足z1≤z3+n‑y3≤z4， 则结束针对本次终点 Q的路径规划。 5.如权利要求1所述的异形空间内的机械手安全轨迹规划方法， 其特征在于， 所述在T 点与Q点之间进行 数字积分圆弧插补运动至Q 点取物， 包括： 由于Y轴向下为正， 所以圆弧轨迹为第一象限逆时针圆弧， 假设点D(zD， yD)为顺圆弧TQ 上一动点， 起点为T， 终点为Q， 圆弧TQ上动点D的速度为v， 在两个坐标轴方向的分速度为vz， vy， 根据三角形相似关系， 可 得到 则有增量：权　利　要　求　书 1/2 页 2 CN 115401687 A 2式中， R为圆弧TQ的半径， K为比例系数， Δz为动点D在Z轴上的位移增量， Δy为动点D在 Y轴上的位置增量， Δt为时间增量； 在数字积分的圆弧插补过程中用两个积分器来实现， 当Δt＝1， K＝1/2N时： 式中， n为总累加次数， zi、 yi为第i次累加时积分器在Z轴和Y轴方向溢出的脉冲个数， Δ zi、 Δyi为第i次累加时动点D在Z轴和Y轴上的位移增量， N为累加器和被积函数寄存器的位 数。 6.如权利要求5所述的异形空间内的机械手安全轨迹规划方法， 其特征在于， 所述进行 数字积分圆弧插补时存在偏差d如下： 当偏差d大于阈值 时， 对轨迹进行修 正， 如下： 在动点D当前位置， 预先计算下一步的偏差， 选择下一步的最优走法， 在Y ‑Z平面， 下一 步可以有三种走法： 往‑Z方向走一 步， 则新的偏差dz为： 往+Y方向走一 步， 则新的偏差dy为： 同时往‑Z方向和+Y方向走一 步， 则新的偏差dzy为： 比较偏差dz、 dy和dzy， 选择偏差最小的方向进给， 以获得最小插补偏差； 继续判断当前位置的偏差是否大于阈值 若仍大于阈值 则继续对轨迹进行修正； 否 则由当前位置继续以数字积分法做圆弧插补。权　利　要　求　书 2/2 页 3 CN 115401687 A 3