(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210902349.9 (22)申请日 2022.07.29 (71)申请人 山东亚历山大智能科技有限公司 地址 250061 山东省济南市历下区经十路 17923号山东大学千佛山校区创新大 厦西楼4层 (72)发明人 周军　付周　赵厚勇　孟广辉　 李文广　高新彪　叶丰　 (74)专利代理 机构 济南圣达知识产权代理有限 公司 372 21 专利代理师 祖之强 (51)Int.Cl. B25J 9/16(2006.01) B25J 11/00(2006.01) (54)发明名称 用于高频接触物体消毒的四轴机械臂运动 控制方法及系统 (57)摘要 本发明提供了一种用于高频接触物体消毒 的四轴机械臂运动控制方法及系统， 属于机器人 控制技术领域； 将空间坐标通过运动学逆解由笛 卡尔坐标转到机械臂坐标系得出机械臂各轴角 度； 将目标位置各轴角度与当前角度作差求出各 轴转角， 将各轴转角转换成电机转角， 根据电机 转角大小按比例分配电机转速； 进行运动学正解 估计机械臂末端的笛卡尔坐标， 比较 笛卡尔坐标 在水平面上的投影是否越过障碍物点构成的约 束， 如果是， 则判断机械臂在运动过程中会碰到 障碍物， 进行路径规划以规避障碍物； 否则， 直接 根据逆解出的各轴转角 控制机械臂到达目标位 置进行消毒； 本发明实现了四轴机械臂的更精准 控制， 进而保证了高频接触物体的消杀精度和效 率。 权利要求书2页 说明书8页 附图4页 CN 114952870 A 2022.08.30 CN 114952870 A 1.一种用于高频接触物体消毒的四轴机 械臂运动控制方法， 其特 征在于： 包括以下 过程： 获取消毒目标的中心位置的空间坐标； 将空间坐标通过运动学逆解由笛卡尔坐标转到 机械臂坐标系得 出机械臂各轴角度； 将目标位置各轴角度与当前角度作差求出各轴转角， 将各轴转角转换成电机转角， 根 据电机转角大小 按比例分配电机转速； 进行运动学正解估计机械臂末端的笛卡尔 坐标， 比较笛卡尔 坐标在水平面上的投影是 否越过障碍物点构成的约束， 如果是， 则判断机械臂在运动过程中会碰到障碍物， 进 行路径 规划以规避障碍物； 否则， 直接根据逆解出的各轴转角控制机 械臂到达目标位置进行消毒。 2.如权利要求1所述的用于高频接触物体消毒的四轴机械臂运动控制方法， 其特征在 于： 所述路径规划， 包括： 将栅格地图上的障碍物进行膨化得到膨化曲线， 以膨化曲线作为机械臂末端的安全工 作界限； 在障碍物膨化曲线、 原路径非碰撞段、 机械臂第一轴旋转中心与目标点连线线段位于 膨化曲线与目标点之间的部分上选取点作为路径点； 用选出来的路径点进行二次B样条曲线拟合， 进行插值选取新的路径点。 3.如权利要求1所述的用于高频接触物体消毒的四轴机械臂运动控制方法， 其特征在 于： 将各轴转角转换成电机转角， 包括： 将各轴转角根据传动比转换成电机转角； 或者， 将其中某个转轴电机需要转动的角度 分割成多个子角度， 计算出该轴电机转到该子角度时其他电机的角度， 根据比例关系 得到 机械臂其他各轴角度。 4.如权利要求1所述的用于高频接触物体消毒的四轴机械臂运动控制方法， 其特征在 于： 进行运动学正 解估计机 械臂末端的笛卡尔坐标， 包括： 通过预建立的二维栅格地图将障碍物相对于世界坐标系XwYw的坐标信息以点云的方 式记录在栅格地图中； 实时获取机器人在栅格地图中的定位， 得到世界坐标系与机器人坐 标系之间的关系Twr， 进 而得到障碍物点云相对于自身的坐标信息XrYr； 根据机械臂和机器人中心的关系， 得到机器人坐标系XrYr到机械臂坐标系XmYm之间的 变换关系Trm， 将障碍物点云的坐标转换到相对于机械臂原 点的笛卡尔转标系上， 得到机械 臂末端的笛卡尔坐标。 5.如权利要求1所述的用于高频接触物体消毒的四轴机械臂运动控制方法， 其特征在 于： 根据电机速度到零之后的当前编码器值与下发的期望编码器值之前的差值是否在误 差允许范围之内判断机 械臂是否 到达目标位置， 在到 达位置之后再进行消毒操作。 6.如权利要求1所述的用于高频接触物体消毒的四轴机械臂运动控制方法， 其特征在 于： 对于单目标点， 没有碰撞直接执行当前位置到目标位置的连续转动， 到达位置完成消权　利　要　求　书 1/2 页 2 CN 114952870 A 2毒动作之后原路返回， 有碰撞则进行运动规划分割得到新的路径点， 依次执行新的路径点， 达到目标位置后原路 返回。 7.如权利要求1所述的用于高频接触物体消毒的四轴机械臂运动控制方法， 其特征在 于： 对于多目标点， 将各目标点进行运动学逆解之后， 将各轴角度存储下来依次执行， 直到 执行完最后一个目标点的位置之后才回零， 不需要原路返回， 在当前位置以机械臂起始位 置的各轴角度为目标位置， 进行碰撞检测 和运动规划。 8.如权利要求1所述的用于高频接触物体消毒的四轴机械臂运动控制方法， 其特征在 于： 对横式门把手内侧进行消毒时， 将识别到的目标点的坐标向上偏移大于四轴的坐标， 求逆解时将四轴机械臂 运动简化为求三轴机械臂逆解， 求出三轴末端到达目标点上方的逆 解， 到达位置后， 将四轴旋转使四轴朝下进行消毒； 对高频接触物体进行消毒时， 对目标位置坐标沿机械臂坐标系的Ym负方向偏移， 然后 求解三个轴的运动学逆解后， 让机 械臂四轴保持水平进行喷洒。 9.一种用于高频接触物体消毒的四轴机 械臂运动控制系统， 其特 征在于： 包括以下 过程： 数据获取模块， 被 配置为： 获取消毒目标的中心位置的空间坐标； 坐标转换模块， 被配置为： 将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂坐标 系得出机械臂各轴角度； 转角计算模块， 被配置为： 将目标位置各轴角度与当前角度作差求出各轴转角， 将各轴 转角转换成电机转角， 根据电机转角大小 按比例分配电机转速； 消毒控制模块， 被配置为： 进行运动学正解估计机械臂末端的笛卡尔 坐标， 比较笛卡尔 坐标在水平面上 的投影是否越过障碍物点构成的约束， 如果是， 则判断机械臂在运动过程 中会碰到障碍物， 进 行路径规划以规避障碍物； 否则， 直接根据逆解出的各轴转角控制机械 臂到达目标位置进行消毒。 10.如权利要求9所述的用于 高频接触物体消毒的四轴机械臂运动控制系统， 其特征在 于： 将栅格地图上的障碍物进行膨化得到膨化曲线S （虚线） ， 作为机械臂末端的安全工作 界限； 从原路径与膨化曲线交点X0开始沿着膨化曲线向着目标点的方向以一定的步长选取 路径点X1， 进行从X1点到目标点的碰撞检测， 如不发生碰撞， 就停止路径点的寻找， 避障路 径就是起始点到X0再到X1最后到目标点； 如果发生碰撞， 就从X1重 复步骤2， 直到找到的路径点Xi到目标点的过程中直接不发生 碰撞； 将选出来的点进行二 次B样条曲线进行拟合， 使路径平缓， 然后进行插值选取新的路径 点， 对于有的路径点曲率比较大， 则将该路径点舍去。权　利　要　求　书 2/2 页 3 CN 114952870 A 3