(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210628203.X (22)申请日 2022.06.06 (71)申请人 南京佗道医疗科技有限公司 地址 210000 江苏省南京市雨 花台区大周 路34号3幢 (72)发明人 程敏　杨辉　申登伟　袁文　 (74)专利代理 机构 南京瑞弘专利商标事务所 (普通合伙) 32249 专利代理师 梁天彦 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种保持末端工具位姿的方法 (57)摘要 本发明公开了一种保持末端工具位姿的方 法， 包括： 建立末端工具的运动学模型， 确定约束 方程； 根据末端工具各关节位置计算末端工具的 受约束位姿； 更新末端工具中运动关节运动后的 约束方程， 结合末端工具的受约束位姿计算得到 其他关节的目标位置， 并驱动对应关节运动。 本 发明通过确定受约束或保持固定的位姿， 建立包 含末端工 具各关节位置变量的约束方程， 当其中 一个或多个关节发生运动时， 通过即采用解析法 或数值法求解其他关节的目标位置； 同时通过 实 时计算关节 运动后的实际位姿， 判断与约束位姿 偏差是否过大， 以此来保证 保持末端工具位姿 过 程中的安全性。 权利要求书1页 说明书4页 附图2页 CN 115139299 A 2022.10.04 CN 115139299 A 1.一种保持末端工具位姿的方法， 其特 征在于： 包括： 建立末端工具的运动学模型， 确定约束方程； 根据末端工具 各关节位置计算末端工具的受约束位姿； 更新末端工具中运动关节运动后的约束方程， 结合末端工具的受约束位姿计算得到其 他关节的目标位置， 并驱动对应关节运动。 2.根据权利要求1所述的保持末端工具位姿的方法， 其特 征在于： 还 包括步骤： 根据运动关节和其 他关节的位置计算得到末端工具的实际位姿； 计算末端工具的实际位姿与受约束位姿的偏差， 并在偏差超出设定 阈值范围时停止各 关节的驱动控制。 3.根据权利要求1所述的保持工具位姿的方法， 其特征在于： 根据 所述末端工具需保持 固定的自由度确定约束方程。 4.根据权利要求1所述的保持工具位姿的方法， 其特征在于： 所述运动关节和其他关节 均包含若干 关节， 其中所述运动关节中的一个或多个关节由于轨迹控制或外力控制发生移 动。权　利　要　求　书 1/1 页 2 CN 115139299 A 2一种保持末端工具位姿的方 法 技术领域 [0001]本发明涉及机器人技 术领域， 尤其涉及一种保持末端工具位姿的方法。 背景技术 [0002]越来越多的装置被电子装置所替代， 例如医疗机器人应用于医院、 诊所的医  疗或 辅助医疗中， 实现半自主或全自主的操作， 它能够完成有益于人类健康的服  务工作。 [0003]在人机协作、 手术机器人等领域中， 末端的工具一般被用于定位、 抓取、 搬  运等操 作， 且通常会设计冗余关节， 以保证末端工具操作的灵活性。 当冗余关节  存在时， 可获得多 种不同的关节位置组合来保证末端工具保持固定位姿。 这是零  空间概念， 即关节运动时， 保证末端位姿固定的技 术。 在实际应用过程中， 可以  选择需要保持的位置和姿态。 [0004]现有技术中， 首先确定末端工具实际变换位姿与参考变换位姿之间的差异，  采用 逆雅可比行列式求解各关节的映射相对误差， 以补偿关节的移动来保证末端  工具位姿固 定。 在一些示例中， 补偿关节误差也被称作关节速度。 此方法在位姿  误差较大时， 同一个微 分间隔求解的关节速度误差会增大， 精度无法保证， 且当  误差越大时， 补偿的精度越差。 与 此同时， 工具位姿由于误差的改变可能导致邻  近工具的人员的损伤、 邻近工具的设备的损 坏等其他不期望后果。 发明内容 [0005]发明目的： 针对上述不足， 本发明提出一种保持末端工具位姿的方法， 精度  不受 位姿误差大小的影响， 同时可以避免对人员或设备的损伤。 [0006]技术方案： [0007]一种保持末端工具位姿的方法， 包括： [0008]建立末端工具的运动学模型， 确定约束方程； [0009]根据末端工具 各关节位置计算末端工具的受约束位姿； [0010]更新末端工具中运动关节运动后的约束方程， 结合末端工具的受约束位姿计  算 得到其他关节的目标位置， 并驱动对应关节运动。 [0011]还包括步骤： [0012]根据运动关节和其 他关节的位置计算得到末端工具的实际位姿； [0013]计算末端工具的实际位姿与受约束位姿的偏差， 并在偏差超出设定阈值范围  时 停止各关节的驱动控制。 [0014]根据所述末端工具需保持固定的自由度确定约束方程。 [0015]所述运动关节和其他关节均包含若干关节， 其中所述运动关节中的一个或多  个 关节由于 轨迹控制或外力控制发生移动。 [0016]有益效果： [0017]1、 本发明通过确定受约束或保持固定的位姿， 建立包含末端工具各关节位  置变 量的约束方程， 当其中一个或多个关节发生运动时， 通过解析法或数值法求  解其他关节的说　明　书 1/4 页 3 CN 115139299 A 3