(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221072891 1.0 (22)申请日 2022.06.24 (71)申请人 南京佗道医疗科技有限公司 地址 210000 江苏省南京市雨 花台区大周 路34号3幢 (72)发明人 程敏　杨辉　薛琪　申登伟　袁文　 (74)专利代理 机构 南京瑞弘专利商标事务所 (普通合伙) 32249 专利代理师 梁天彦 (51)Int.Cl. A61B 34/37(2016.01) B25J 9/16(2006.01) (54)发明名称 保持末端位姿固定的让位方法 (57)摘要 本发明公开了保持工具位姿固定的让位方 法， 包括步骤： 建立让位臂运动学模型； 根据让位 需求得到在让位过程中让位臂末端的受约束位 姿和非约束位姿， 根据让位臂的运动自由度及其 受约束位姿对应自由度得到的冗余自由度构建 用于驱动其冗余自由度对应关节的虚拟轴； 计算 在虚拟轴驱动下的让位臂各实际关节 位置， 据此 驱动让位臂对应 关节运动。 本发 明构建约束虚拟 轴， 用以保证让位过程的虚拟轴速度均匀性， 避 免了以关节驱动导致其他关节速度变化过大的 问题。 权利要求书1页 说明书5页 附图5页 CN 114948238 A 2022.08.30 CN 114948238 A 1.保持末端位姿固定的让位方法， 其特 征在于： 包括 步骤： 建立让位臂运动学模型； 确定让位过程中让位臂末端的受约束位姿和非约束位姿， 构建用于驱动非约束位姿中 的一个或多个自由度对应关节的虚拟轴； 计算在虚拟轴驱动下的让位臂各实际关节的目标位置， 据此驱动让位臂对应关节运 动。 2.根据权利要求1所述的保持末端位姿固定的让位方法， 其特征在于： 根据让位臂末端 需保持固定的自由度， 得到让位臂末端在让位过程中的受约束位姿和非约束位姿。 3.根据权利要求2所述的保持末端位姿固定的让位方法， 其特征在于： 所述虚拟轴的自 由度数为让位臂的运动自由度与其末端需保持固定的自由度数量之差 。 4.根据权利要求1所述的保持末端位姿固定的让位方法， 其特 征在于： 还 包括步骤： 在驱动让位臂对应关节运动后， 计算让位臂末端的实 际位姿， 比对其与让位臂末端的 受约束位姿之间的误差， 并在二 者误差超出阈值停止控制对应关节运动。 5.根据权利要求4所述的保持末端位姿固定的让位方法， 其特征在于： 计算所述让位臂 末端的实际位姿和受约束位姿的各对应自由度位姿之 间的误差， 若二者各对应自由度位姿 之间的误差均在阈值范围内， 则下发驱动器控制对应关节运动； 否则停止控制关节运动。 6.根据权利要求5所述的保持末端位姿固定的让位方法， 其特征在于： 所述阈值范围具 体为： 平移自由度设为0.1m m， 旋转自由度设为0.0 5rad。 7.根据权利要求1所述的保持末端位姿固定的让位方法， 其特征在于： 计算在虚拟轴驱 动下的让位臂各实际关节的目标位置具体为： 构建在虚拟轴驱动下 让位臂各关节位置和让位臂末端的受约束位姿的约束方程组； 求解约束方程组得到让位臂各关节的目标位置， 并据此控制对应关节运动。 8.根据权利要求1所述的保持末端位姿固定的让位方法， 其特征在于： 构建虚拟轴后， 判断该虚拟轴 是否与让位臂各实际关节之间是否存在约束关系， 若不存在， 则重新选取非 约束位姿中的其 他自由度对应的关节构建虚拟轴。 9.根据权利要求1所述的保持末端位姿固定的让位方法， 其特征在于： 构建虚拟轴θv具 体为： θv＝θ0‑kθkt 其中， θ0为在非约束位姿中选取的自由度关节的初始值； θk为单位时间的转动角度， t为 驱动时间， k 为所述虚拟轴与待驱动的自由度关节之间的驱动比例。权　利　要　求　书 1/1 页 2 CN 114948238 A 2保持末端位姿 固定的让位方 法 技术领域 [0001]本发明涉及机器人技 术领域， 尤其涉及一种保持末端位姿固定的让位方法。 背景技术 [0002]机器人辅助手术系统通常包含安装有内镜的持镜臂和安装有手术仪器的多个仪 器臂。 在机器人辅助手术过程中， 医生通过主从方式控制主手来遥控各臂的动作来实现手 术。 系统中各臂之间在手术过程中会发生相对运动， 当各臂初始 位置放置不合理时， 会导致 臂之间的相互碰撞。 因此， 需要在手术前对各臂的位姿进行调整。 通常需要保证臂件末端指 定位姿固定情况下， 将各臂件并拢定位并隔开一定距离， 确保有充足手术空间以供各臂平 行工作和避免可能的臂件碰撞。 [0003]现有方法以臂件的指定关节作为驱动源， 但移动关节和转动关节在保持末端位姿 固定情况下匀速让位时的控制量比例存在较大差异， 若以其中一个关节作为匀速驱动， 则 其他关节速度存在过大或过小的可能。 如臂件的多个连杆平行或垂直时， 构型会接近奇异 位姿， 当驱动关节匀速运动时， 会导 致其他关节速度变化过小或过 大， 存在失控的风险。 发明内容 [0004]发明目的： 本 发明针对不足， 提出一种保持末端位姿固定的让位方法， 保持让位过 程的速度均匀 性， 并且保持末端位姿精度高， 避免了以关节驱动导致其他关节速度变化过 大的问题。 [0005]技术方案： [0006]保持末端位姿固定的让位方法， 包括 步骤： [0007]建立让位臂运动学模型； [0008]确定让位过程中让位臂末端的受约束位姿和非约束位姿， 构建用于驱动非约束位 姿中的一个或多个自由度对应关节的虚拟轴； [0009]计算在虚拟轴驱动下的让位臂各实际关节的目标位置， 据此驱动让位臂对应关节 运动。 [0010]根据让位臂末端需保持固定的自由度， 得到让位臂末端在让位过程中的受约束位 姿和非约束位姿。 [0011]所述虚拟轴的自由度数为让位臂的运动自由度与其末端需保持固定的自由度数 量之差。 [0012]还包括步骤： [0013]在驱动让位臂对应关节运动后， 计算让位臂末端的实际位姿， 比对其与让位臂末 端的受约束位姿之间的误差， 并在二 者误差超出阈值停止控制对应关节运动。 [0014]计算所述让位臂末端的实际位姿和受约 束位姿的各对应自由度位姿之间的误差， 若二者各对应 自由度位姿之间的误差均在阈值范围内， 则下发驱动器控制对应关节运动； 否则停止控制关节运动。说　明　书 1/5 页 3 CN 114948238 A 3