(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210443330.2 (22)申请日 2022.04.25 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 付明磊　徐涛　张文安　刘锦元　 仇翔　刘安东　杨旭升　史秀纺　 周叶剑　吴麒　胡佛　 (74)专利代理 机构 杭州天正专利事务所有限公 司 33201 专利代理师 舒良 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种用于医护机 器人的柔 顺控制方法 (57)摘要 一种用于医护机器人的柔顺控制方法， 包括 以下步骤： 步骤1： 依据期望的机器人末端执行器 的位移值与环境的实际作用力以及设定的导纳 参数， 通过外环导纳控制将外部接触力转化为位 置修正量Xc(t)； 步骤2： 采用滑膜控制来构建内 环控制方法得到对应的内环反馈控制力矩τ， 并 将其输入 给电机系统进行控制， 实现医护机器人 关节的柔顺控制。 本发明能够 有效在医护机器人 护理病人时， 与病人进行柔性的物理接触或者抓 取外界柔性物体时， 具有一定的柔顺性， 保证医 护机器人与人接触时的安全性与鲁棒性， 防止对 病人以及机 械臂造成伤害与破坏。 权利要求书2页 说明书5页 附图2页 CN 114700951 A 2022.07.05 CN 114700951 A 1.一种用于医护机器人的柔 顺控制方法， 包括以下步骤： 步骤1： 依据期望的机器人末端执行器的位移值与环境的实 际作用力以及设定的导纳 参数， 通过外环导纳控制将外 部接触力转 化为位置修正量Xc(t)； 具体包括： S1:根据患 者实际需求使得医护机器人通过运动规划算法规划出末端执行器的目标位 置； S2:在医护机器人到达目标位置时与环境进行物理接触获取末端实时力， 将其与预设 的期望力相比得到末端力偏差； S3:将S2中获取到的末端偏差力通过外环导纳控制器进行校正， 得到机械臂末端 的位 置修正， 并获取到新的目标位置； 步骤2： 采用滑膜控制来构建内环控制方法得到对应的内环反馈控制力矩τ， 并将其输 入给电机系统进行控制， 实现医护机器人关节的柔 顺控制； 具体包括： T1:将医护机器人获取到的当前末端位置信息与S3中获取到的新的位置信息一同传入 内环滑膜控制器， 得到医护机器人末端的控制力矩； T2： 将医护机器人末端控制力矩发送给电机系统， 实现医护机器人的柔 顺控制。 2.如权利要求1所述的一种用于 医护机器人的柔顺控制方法， 其特征在于： 步骤S3所述 的外环导纳控制器具体包括： S3.1:依据期望的机器人末端执行器的位移值与环境的实际作用力以及设定的导纳参 数， 通过外环导纳控制将外部接触力 矩转化为位置修正量Xc(t)， 并以此构建导纳控制的模 型： 其中Sm、 Dm、 Km分别表示导纳控制模型的惯性对角矩阵、 阻尼对角矩阵以及刚度正定对 交矩阵； Fj(t)、 Fi(t)、 Fe(t)分别表示在笛卡尔坐标系中的医护机器人末端执行器所获得的 期望交互力、 实际交互力以及实际与期望的偏差力； Xi(t)、 X(t)以及Xc(t)分别表示笛卡尔 坐标系中医护机器人末端执行器的期望运动轨迹、 目标控制轨迹以及轨迹应调量； 分别表示轨 迹应调整量对时间的二阶导与一阶导； S3.2:在频域中使用拉普拉斯变换重新描述式(1)可 得： 通过步骤1中的外环导纳控制能够建立出力与位移 的动态关系， 且能够通过改变末端 执行器的位移量来限制机器人与 物体交互时的接触力 在期望力周围波动， 然而导纳控制无 法实现对运动目标的轨迹跟踪， 因此本发明通过选择模糊滑膜控制来构建内环控制方法实 现对机械臂末端执 行器位移的跟踪。 3.如权利要求1所述的一种用于 医护机器人的柔顺控制方法， 其特征在于： 步骤T1所述 的内环滑模控制具体包括： T1.1:设计滑模 控制模块： 定义各滑动模块轨 迹误差e(t)为：权　利　要　求　书 1/2 页 2 CN 114700951 A 2e(t)＝K(Ti(t)‑T(t))                             (3) 其中T为实际的返回轨 迹， Ti为期望的控制轨 迹， K为比例系数； T1.2:定义滑模切换函数H(t)为： 其中Q为比例正定对角矩阵； T1.3:通过符号函数sgn()设计滑模控制律 其中M为切 换函数的增 益正定对角矩阵， 其元 素值通过模糊控制器进行调整， 可 得： T1.4:因此， 上式即为本发明所设计的滑模控制器， 通过机器人拉格朗日动力学方程一 般表现形式(6)将其 等价变形为加速度与力矩的直接表现形式(7)； 其中， TL分别为笛卡尔坐标系中广义坐标的广义加速度， 速度以及位置， JM、 JC、 JG 分别表示机器人的惯性力矩、 离心力矩以及重力矩， JD、 JK、 FL、 EL分别表示机器人的雅克比 矩阵、 电机力矩转换矩阵、 机器人末端执行器各坐标轴分力以及电机驱动力矩， 其中 τ1、 τ2、 τ2分别为电机空间中的三维力矩； T1.5:令TL等于T， 将(7)代入(5)中可 得： T1.6:经过等式变换， 最终可得滑模控制率的最终待控制关节力矩与加速度的表现形 式为： 最终， 通过滑模 控制率输出控制力矩， 实现对医护机器人的柔 顺控制。权　利　要　求　书 2/2 页 3 CN 114700951 A 3