一种遥操作的零力控制方法与流程

本发明涉及机器人，尤其涉及一种遥操作的零力控制方法。

背景技术：

1、目前微创手术已基本取代开放手术成为外科医学领域发展的主要方向。相比于传统的开放式手术，微创手术具有创伤小、病痛轻、恢复快等优势。随着机器人技术的发展，基于腹腔镜手术机器人的微创手术逐渐成熟，并被广泛应用。

2、腹腔镜手术机器人为主从遥操作式结构，包括主手和从臂，医生通过操作主手来控制从臂的末端器械和内窥镜的运动。主从操作的基本流程为：在主手与从臂的末端器械的位姿进行匹配后，由医生手持主手的末端执行器进行操作，控制器实时将主手的末端执行器的位姿映射到从臂的末端器械，末端器械实时复现主手的位姿。

3、在手术操作过程中，医生会不断地抓持主手末端执行器，改变主手末端的位姿。由于医生手部的运动范围较大，通常会将主手的运动范围设置得较大，使得医生的操作变得舒适。但由于从臂的末端器械存在机械限位，其运动空间较小。因此当医生操作主手使得从臂的末端器械超出其运动空间时，末端器械将暂停运动，导致手术中断，大大地延长了手术时间，同时也使得医生的操作体验变差，遥操作的沉浸感变差。

技术实现思路

1、发明目的：基于此需求，有必要发明一种符合人体操作直觉的遥操作的零力控制方法，不会中断手术流程，缩短了手术时间，遥操作的沉浸感更强。

2、技术方案：

3、一种遥操作的零力控制方法，包括：

4、获取主手末端的实时位姿并映射至从臂末端，计算得到从臂各关节的期望位置，并控制从臂各关节运动；

5、获取当前器械对应从臂各关节的限位范围，实时获取从臂各关节位置，判断其是否在所述限位范围内，并对不在所述限位范围内的关节进行阻抗控制，计算主手各关节的期望力矩并进行控制。

6、所述阻抗控制具体为：

7、构建阻抗控制器，得到对应关节的阻抗力τ为：

8、τ＝kp*(qm-qt)+kd*(0-vt)

9、其中，kp、kd分别为阻抗控制器的刚度增益矩阵和阻尼矩阵；vt为该关节的当前速度，从该关节的电机编码器读取得到；qm为该关节的期望位置，qt为该关节的当前位置。

10、kp＝mw2，kd＝2kmw；其中，m为主手的惯量，w为自然频率，k为阻尼比。

11、所述关节的期望位置的获取具体为：

12、若实时获取的关节位置大于其工作的限位范围的最大值，则设置其期望位置为该最大值；

13、若实时获取的关节位置小于其工作的限位范围的最小值，则设置其期望位置为该最小值。

14、所述计算主手各关节的期望力矩具体为：

15、根据所述从臂各关节位置计算对应关节的雅克比矩阵，结合所述阻抗控制中的阻抗力计算从臂各关节的笛卡尔空间力矩，并将其映射至主手；

16、根据主手各关节的当前位置计算得到其对应关节的雅克比矩阵，结合通过机器人逆向动力学计算得到的主手各关节的重力矩，计算得到主手各关节的期望力矩。

17、所述获取主手末端的实时位姿并映射至从臂末端具体为：通过机器人正向运动学实时计算当前的主手末端相对于其基座的位姿，并映射至从臂末端，计算得到从臂末端相对于其基座的期望位姿。

18、分别在主手基座和从臂基座建立指向相同的坐标系，据此得到主手末端和从臂末端之间的映射关系。

19、所述获取当前器械对应从臂各关节的限位范围具体为：

20、根据从臂末端器械的类型，获取当前器械运动至其极限位置时从臂各关节的最大值和最小值，作为当前器械工作的限位范围。

21、设计一缓冲值e，计算得到从臂各关节运动的最大限位qmax＝qmax-e和最小限位qmin＝qmin+e，得到当前器械工作的限位范围。

22、所述缓冲值e的取值范围为(0，10°]。

23、所述e＝5°。

24、在所述器械上设有rfid设备以供读取该器械的参数信息，所述参数信息包括当前器械的类型及其极限位置。

25、有益效果：本发明通过模拟虚拟的碰撞环境来计算相应的虚拟的碰撞力，将虚拟的碰撞力换算到主手端电机下发，医生不但能感受器械机械限位的碰撞力，主手也不会发生医生不期望的运动，不会中断手术流程，缩短了手术时间，同时大大提高医生的操作体验，遥操作的沉浸感更强。

技术特征：

1.一种遥操作的零力控制方法，其特征在于，包括：

2.根据权利要求1所述的遥操作的零力控制方法，其特征在于，所述阻抗控制具体为：

3.根据权利要求2所述的遥操作的零力控制方法，其特征在于，kp＝mw2，kd＝2kmw；其中，m为主手的惯量，w为自然频率，k为阻尼比。

4.根据权利要求2或3所述的遥操作的零力控制方法，其特征在于，所述关节的期望位置的获取具体为：

5.根据权利要求1所述的遥操作的零力控制方法，其特征在于，所述计算主手各关节的期望力矩具体为：

6.根据权利要求1所述的遥操作的零力控制方法，其特征在于，所述获取主手末端的实时位姿并映射至从臂末端具体为：通过机器人正向运动学实时计算当前的主手末端相对于其基座的位姿，并映射至从臂末端，计算得到从臂末端相对于其基座的期望位姿。

7.根据权利要求6所述的遥操作的零力控制方法，其特征在于，分别在主手基座和从臂基座建立指向相同的坐标系，据此得到主手末端和从臂末端之间的映射关系。

8.根据权利要求1所述的遥操作的零力控制方法，其特征在于，所述获取当前器械对应从臂各关节的限位范围具体为：

9.根据权利要求8所述的遥操作的零力控制方法，其特征在于，设计一缓冲值e，计算得到从臂各关节运动的最大限位qmax＝qmax-e和最小限位qmin＝qmin+e，得到当前器械工作的限位范围。

10.根据权利要求9所述的遥操作的零力控制方法，其特征在于，所述缓冲值e的取值范围为(0，10°]。

11.根据权利要求10所述的遥操作的零力控制方法，其特征在于，所述e＝5°。

12.根据权利要求8所述的遥操作的零力控制方法，其特征在于，在所述器械上设有rfid设备以供读取该器械的参数信息，所述参数信息包括当前器械的类型及其极限位置。

技术总结
本发明公开了一种遥操作的零力控制方法，包括：获取主手末端的实时位姿并映射至从臂末端，计算得到从臂各关节的期望位置，并控制从臂各关节运动；获取当前器械对应从臂各关节的限位范围，实时获取从臂各关节位置，判断其是否在所述限位范围内，并对不在所述限位范围内的关节进行阻抗控制，计算主手各关节的期望力矩并进行控制。本发明的操作过程中，医生不但能感受器械机械限位的碰撞力，主手也不会发生医生不期望的运动，不会中断手术流程，缩短了手术时间，同时大大提高医生的操作体验，遥操作的沉浸感更强。

技术研发人员：陈云川,桂凯,杨辉
受保护的技术使用者：佗道医疗科技有限公司
技术研发日：
技术公布日：2024/1/11