(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210687904.0 (22)申请日 2022.06.16 (71)申请人 法奥意威(苏州)机 器人系统有限公 司 地址 215000 江苏省苏州市高新区竹园路 209号2号楼1810室 (72)发明人 赵进奇　丁磊　姚庭　王超　 史琦亮　高加超　 (74)专利代理 机构 北京超凡宏宇专利代理事务 所(特殊普通 合伙) 11463 专利代理师 杜杨 (51)Int.Cl. B25J 9/02(2006.01) B25J 9/08(2006.01) B25J 9/16(2006.01) (54)发明名称 约束运动控制方法、 装置、 系统和电子设备 (57)摘要 本申请提供一种约束运动控制方法、 装置、 系统和电子设备， 针对配置有工具的机器人， 获 得工具所在的工具坐标系相对于机器人末端所 在的末端坐标系的变换矩阵， 并标定远心不动 点， 获得目标位姿信息。 根据目标位姿信息及变 换矩阵得到工具的末端位姿， 根据工具的末端位 姿中的末端位置信息及标定的远心不动点对变 换矩阵进行更新。 最后基于更新后的变换矩阵得 到机器人各关节的角度位置信息， 进而控制实体 机器人带动配置的实体工具进行远心不动点约 束的运动。 本方案基于工具末端、 机器人末端、 远 心不动点之间的相对关系实现约束控制， 无需配 套的机械 结构， 减少了维护成本并且降低对手术 环境污染的可能性， 并且还可避免机械结构件之 间连接时产生的误差 。 权利要求书2页 说明书12页 附图4页 CN 114952806 A 2022.08.30 CN 114952806 A 1.一种约束运动控制方法， 其特 征在于， 所述方法包括： 针对配置有工具的机器人， 获得所述工具所在的工具坐标系相对于所述机器人末端所 在的末端坐标系的变换矩阵； 标定得到远心不动点， 并获得目标位姿信息； 根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿； 根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进 行更新； 根据更新后的变换矩阵得到机器人各关节的角度位置信 息， 并基于所述角度位置信 息 控制实体机器人 带动配置的实体工具进行远心不动点约束的运动。 2.根据权利要求1所述的约束运动控制方法， 其特征在于， 所述方法还包括预先配置工 具信息的步骤， 该步骤 包括以下之一： 从预先构建的多个工具模型中选择至少一个工具模型， 获得选择的至少一个工具模型 的工具信息， 基于所述工具信息在所述机器人的末端 进行工具配置； 或 获取以参数设置方式产生的工具信 息， 基于所述工具信 息在所述机器人的末端进行工 具配置。 3.根据权利要求1所述的约束运动控制方法， 其特征在于， 所述获得所述工具所在的工 具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵的步骤， 包括： 在所述机器人处于多种不同姿态下， 控制所述工具的末端与空间中一设置的坐标点重 合， 并记录各种姿态下 所述机器人末端的空间笛卡尔坐标 数据； 根据多种不同姿态下所述机器人末端的空间笛卡尔坐标数据计算得到所述工具所在 的工具坐标系相对于所述机器人末端 所在的末端坐标系的空间笛卡尔坐标， 以完成所述工 具坐标系的标定； 根据已完成标定的工具坐标系， 获得所述工具坐标系相对于所述末端坐标系的变换矩 阵。 4.根据权利要求1所述的约束运动控制方法， 其特征在于， 所述标定得到远心不动点的 步骤， 包括： 在所述机器人处于多种不同姿态下， 控制所述工具的末端与设置的远心不动点重合， 并记录各种姿态下 所述工具的末端的空间笛卡尔坐标 数据； 根据多种姿态下所述工具的末端的空间笛卡尔坐标数据计算得到所述远心不动点相 对于所述机器人的底 座所在的基坐标系下的空间笛卡尔坐标， 以完成所述远心 不动点的标 定。 5.根据权利要求1所述的约束运动控制方法， 其特征在于， 所述获得目标位姿信 息的步 骤， 包括： 获取摇杆被控制下 所产生的偏移量； 在当前位姿信息基础上叠加所述偏移量得到目标位姿信息 。 6.根据权利要求1所述的约束运动控制方法， 其特征在于， 所述变换矩阵包括多个类型 的位姿变换向量和多个方向上的位移信息； 所述根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩 阵进行更新的步骤， 包括：权　利　要　求　书 1/2 页 2 CN 114952806 A 2根据所述工具的末端位姿中的末端位置信息及标定的远心不动点的坐标计算得到新 的位姿变换向量和新的位移信息； 基于所述 新的位姿变换向量和新的位移信息对所述变换矩阵进行 更新。 7.根据权利要求6所述的约束运动控制方法， 其特征在于， 所述根据所述工具的末端位 姿中的末端位置信息及标定的远心不动点的坐标计算得到新的位姿变换向量和 新的位移 信息的步骤， 包括： 计算所述工具的末端位姿中的末端位置信息和标定的远心不动点的坐标中同一方向 上的坐标值的差值， 结合多个方向上的差值计算得到新的位移信息； 根据多个方向上的差值构 成差值向量， 并根据所述差值向量和所述新的位移信 息计算 得到新的位姿变换向量。 8.一种约束运动控制装置， 其特 征在于， 所述装置包括： 变换矩阵获得模块， 用于针对配置有工具的机器人， 获得所述工具所在的工具坐标系 相对于所述机器人末端所在的末端坐标系的变换矩阵； 标定模块， 用于标定得到远心不动点， 并获得目标位姿信息； 末端位姿获得模块， 用于根据 所述目标位姿信 息及所述变换矩阵得到所述工具的末端 位姿； 更新模块， 用于根据 所述工具的末端位姿中的末端位置信 息及标定的远心不动 点对所 述变换矩阵进行 更新； 控制模块， 用于根据更新后的变换矩阵得到机器人各关节的角度位置信息， 并基于所 述角度位置信息控制实体机器人 带动配置的实体工具进行远心不动点约束的运动。 9.一种电子设备， 其特征在于， 包括一个或多个存储介质和一个或多个与存储介质通 信的处理器， 一个或多个存储介质存储有处理器可执行 的机器可执行指令， 当电子设备运 行时， 处理器执行所述机器可 执行指令， 以执 行权利要求1 ‑7中任意一项所述的方法步骤。 10.一种约束运动控制系统， 其特征在于， 包括控制设备、 实体机器人、 实体工具以及电 子设备； 所述电子设备用于， 针对配置有工具的机器人， 获得所述工具所在的工具坐标系相对 于所述机器人末端所在的末端坐标系的变换矩阵， 标定得到远心不动点， 并获得目标位姿 信息， 根据所述目标位姿信息及所述变换矩阵得到所述工具 的末端位姿， 根据所述工具 的 末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新， 根据更新后的 变换矩阵得到 机器人各关节的角度位置信息； 所述电子设备还用于， 将所述角度位置信息发送至所述控制设备； 所述控制设备用于， 根据接收到的角度位置信息控制所述实体机器人和实体工具； 所述实体机器人用于， 在所述控制设备的控制下带动配置的实体工具进行远心不动点 约束的运动。权　利　要　求　书 2/2 页 3 CN 114952806 A 3