(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221081819 2.1 (22)申请日 2022.07.12 (71)申请人 南京佗道医疗科技有限公司 地址 210000 江苏省南京市雨 花台区大周 路34号3幢 (72)发明人 程敏　桂凯　杨辉　申登伟　 陈云川　 (74)专利代理 机构 南京瑞弘专利商标事务所 (普通合伙) 32249 专利代理师 梁天彦 (51)Int.Cl. B25J 9/16(2006.01) A61B 34/35(2016.01) (54)发明名称 基于传感器的机 械臂柔顺控制方法 (57)摘要 本发明公开了一种基于传感器的机械臂柔 顺控制方法， 包括： 根据机械臂的自由度数和传 感器测量维度确定冗余关节； 通过传感器获取机 械臂所受外力； 计算得到机械臂在其所受外力控 制下的期望位姿， 通过运动学逆解解算得到非冗 余关节的控制量， 并据此控制对应关节运动。 本 发明通过拆分冗余关节， 混合控制实现机械臂的 柔顺拖动， 降低系统成本和设计复杂度； 且不对 冗余自由度进行限制， 保证柔 顺拖动的自由度。 权利要求书1页 说明书3页 附图3页 CN 115157252 A 2022.10.11 CN 115157252 A 1.基于传感器的机 械臂柔顺控制方法， 其特 征在于： 包括： 根据机械臂的自由度数和传感器测量维度确定 冗余关节； 通过传感器获取机 械臂所受外力； 计算机械臂在其所受外力控制下的期望位姿， 通过运动学逆解得到非冗余关节的控制 量， 并据此控制对应关节运动。 2.根据权利要求1所述的机械臂柔顺控制方法， 其特征在于： 计算机械臂在其所受外力 控制下的期望位姿具体为： 获取传感器坐标系下机 械臂所受外力控制下的期望位姿； 根据冗余关节坐标系与传感器坐标系之间的变换关系及冗余关节坐标系与机器人基 坐标系之间的变换关系将前述期望位姿变换至 机器人基坐标系下。 3.根据权利要求2所述的机械臂柔顺控制方法， 其特征在于： 选取最靠近手握拖动点的 冗余关节建立冗余关节坐标系。 4.根据权利要求2所述的机械臂柔顺控制方法， 其特征在于： 所述手握拖动 点为传感器 安装的位置， 所述传感器安装在机 械臂的末端。 5.根据权利要求1所述的机械臂柔顺控制方法， 其特征在于： 根据非冗余关节的控制量 控制对应关节运动具体为： 通过位置环的方式控制关节， 使其到 达期望角度。 6.根据权利要求1所述的机械臂柔顺控制方法， 其特征在于： 根据非冗余关节的控制量 控制对应关节运动具体为： 通过电流环 的方式控制关节， 获取机械臂各关节的当前位置， 并根据其期望位姿得到 机械臂各关节的期望位置， 从而得到 机械臂各关节电机的期望电流， 并据此进行控制。 7.根据权利要求1所述的机械臂柔顺控制方法， 其特征在于： 根据机械臂的自由度 数和 传感器测量维度确定 冗余关节具体为： 通过手术机械臂的自由度数和传感器测量维度的差值确定冗余关节数量， 根据冗余关 节为传感器的非直接测量维度且离手 握拖动点更近的原则选取冗余关节。 8.根据权利要求1所述的机械臂柔顺控制方法， 其特征在于： 通过传感器获取机械臂所 受外力后， 采用均值滤波和/或低通滤波的方式对其进行 滤波处理。权　利　要　求　书 1/1 页 2 CN 115157252 A 2基于传感器的机械臂柔顺控制方 法 技术领域 [0001]本发明涉及手术机器人技术领域， 尤其涉及一种基于传感器的机械臂柔顺控制方 法。 背景技术 [0002]机器人辅助手术系统是一套利用腹腔镜、 胸腔镜等医疗器械及相关设备在人体腔 内部实施手术的系统， 一般医生通过主从方式控制主手来遥控各臂的动作来实现手术， 达 到微创的目的。 在手术准备过程中， 需要安装各类手术器械， 这就需要将 手术机械臂移动到 指定的位姿， 相比于按钮或点动控制手术机械臂， 拖动的方式效率更高， 且不需要额外人 力。 [0003]现有实现方式有基于电流环的零力拖动和基于传感器的柔顺控制。 基于电流环的 零力拖动通常会受 限于结构形态和动力学参数特性， 这容易导致在奇异形态， 某些关节无 法受力拖动而影响整体的拖动效果。 柔顺控制是基于传感器反馈的信号来控制 机械臂， 响 应环境力的变化而变化， 这更能影响到医生拖动手术机械臂的主观感受， 让手术机械臂按 医生的拖动意图响应动作。 现有实现柔顺控制的拖动技术中， 一般采用导纳控制， 即在手术 机械臂末端安装传感器， 传感器检测到外部作用力后， 导纳控制器根据外部作用力输出一 个新的期 望位置， 驱动手术臂到期 望位置， 实现柔顺效果。 然而， 为了 保证控制安全性， 手术 机械臂一般会设计冗余结构， 这导致柔顺控制无法直接应用。 目前也有采用增加约束方程 的方式来解算各关节位置， 但 这同时也限制了可拖动手术机械臂的自由度。 另外， 可以增加 传感器的测量维度数， 但随着传感器维度的增加， 价格一般会更高， 且系统的复杂度会增 加。 发明内容 [0004]发明目的： 本 发明针对上述不足， 提出一种基于传感器的机械臂柔顺控制方法， 不 对冗余自由度进行限制， 保证柔 顺拖动的自由度， 可以降低系统成本和设计复杂度。 [0005]技术方案： [0006]基于传感器的机 械臂柔顺控制方法， 包括： [0007]根据机械臂的自由度数和传感器测量维度确定 冗余关节； [0008]通过传感器获取机 械臂所受外力； [0009]计算得到机械臂在 其所受外力控制下的期望位姿， 通过运动学逆解得到非冗余关 节的控制量， 并据此控制对应关节运动。 [0010]计算机械臂在其所受外力控制下的期望位姿具体为： [0011]获取传感器坐标系下机 械臂所受外力控制下的期望位姿； [0012]根据冗余关节坐标系与传感器坐标系之间的变换关系及冗余关节坐标系与机器 人基坐标系之间的变换关系将期望位姿变换至 机器人基坐标系下。 [0013]选取最靠 近手握拖动点的冗余关节建立冗余关节坐标系。说　明　书 1/3 页 3 CN 115157252 A 3