一种遥操作的零力控制方法技术

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

【技术实现步骤摘要】
一种遥操作的零力控制方法


[0001]本专利技术涉及机器人
，尤其涉及一种遥操作的零力控制方法。

技术介绍

[0002]目前微创手术已基本取代开放手术成为外科医学领域发展的主要方向。相比于传统的开放式手术，微创手术具有创伤小、病痛轻、恢复快等优势。随着机器人技术的发展，基于腹腔镜手术机器人的微创手术逐渐成熟，并被广泛应用。
[0003]腹腔镜手术机器人为主从遥操作式结构，包括主手和从臂，医生通过操作主手来控制从臂的末端器械和内窥镜的运动。主从操作的基本流程为：在主手与从臂的末端器械的位姿进行匹配后，由医生手持主手的末端执行器进行操作，控制器实时将主手的末端执行器的位姿映射到从臂的末端器械，末端器械实时复现主手的位姿。
[0004]在手术操作过程中，医生会不断地抓持主手末端执行器，改变主手末端的位姿。由于医生手部的运动范围较大，通常会将主手的运动范围设置得较大，使得医生的操作变得舒适。但由于从臂的末端器械存在机械限位，其运动空间较小。因此当医生操作主手使得从臂的末端器械超出其运动空间时，末端器械将暂停运动，导致手术中断，大大地延长了手术时间，同时也使得医生的操作体验变差，遥操作的沉浸感变差。

技术实现思路

[0005]专利技术目的：基于此需求，有必要专利技术一种符合人体操作直觉的遥操作的零力控制方法，不会中断手术流程，缩短了手术时间，遥操作的沉浸感更强。
[0006]技术方案：
[0007]一种遥操作的零力控制方法，包括：
[0008]获取主手末端的实时位姿并映射至从臂末端，计算得到从臂各关节的期望位置，并控制从臂各关节运动；
[0009]获取当前器械对应从臂各关节的限位范围，实时获取从臂各关节位置，判断其是否在所述限位范围内，并对不在所述限位范围内的关节进行阻抗控制，计算主手各关节的期望力矩并进行控制。
[0010]所述阻抗控制具体为：
[0011]构建阻抗控制器，得到对应关节的阻抗力τ为：
[0012]τ＝K
p
*(q
m
‑
q
t
)+K
d
*(0
‑
v
t
)
[0013]其中，K
p
、K
d
分别为阻抗控制器的刚度增益矩阵和阻尼矩阵；v
t
为该关节的当前速度，从该关节的电机编码器读取得到；q
m
为该关节的期望位置，q
t
为该关节的当前位置。
[0014]K
p
＝mw2，K
d
＝2kmw；其中，m为主手的惯量，w为自然频率，k为阻尼比。
[0015]所述关节的期望位置的获取具体为：
[0016]若实时获取的关节位置大于其工作的限位范围的最大值，则设置其期望位置为该最大值；
[0017]若实时获取的关节位置小于其工作的限位范围的最小值，则设置其期望位置为该最小值。
[0018]所述计算主手各关节的期望力矩具体为：
[0019]根据所述从臂各关节位置计算对应关节的雅克比矩阵，结合所述阻抗控制中的阻抗力计算从臂各关节的笛卡尔空间力矩，并将其映射至主手；
[0020]根据主手各关节的当前位置计算得到其对应关节的雅克比矩阵，结合通过机器人逆向动力学计算得到的主手各关节的重力矩，计算得到主手各关节的期望力矩。
[0021]所述获取主手末端的实时位姿并映射至从臂末端具体为：通过机器人正向运动学实时计算当前的主手末端相对于其基座的位姿，并映射至从臂末端，计算得到从臂末端相对于其基座的期望位姿。
[0022]分别在主手基座和从臂基座建立指向相同的坐标系，据此得到主手末端和从臂末端之间的映射关系。
[0023]所述获取当前器械对应从臂各关节的限位范围具体为：
[0024]根据从臂末端器械的类型，获取当前器械运动至其极限位置时从臂各关节的最大值和最小值，作为当前器械工作的限位范围。
[0025]设计一缓冲值e，计算得到从臂各关节运动的最大限位Q
max
＝q
max
‑
e和最小限位Q
min
＝q
min
+e，得到当前器械工作的限位范围。
[0026]所述缓冲值e的取值范围为(0，10
°
]。
[0027]所述e＝5
°
。
[0028]在所述器械上设有RFID设备以供读取该器械的参数信息，所述参数信息包括当前器械的类型及其极限位置。
[0029]有益效果：本专利技术通过模拟虚拟的碰撞环境来计算相应的虚拟的碰撞力，将虚拟的碰撞力换算到主手端电机下发，医生不但能感受器械机械限位的碰撞力，主手也不会发生医生不期望的运动，不会中断手术流程，缩短了手术时间，同时大大提高医生的操作体验，遥操作的沉浸感更强。
附图说明
[0030]图1为本专利技术的控制流程图；
[0031]图2为主手和从臂对应的坐标系的示意图；
[0032]图3为从臂和主手的运动关系示意图。
[0033]其中，1为主手，2为从臂。
具体实施方式
[0034]下面结合附图和具体实施例，进一步阐明本专利技术。
[0035]图1为本专利技术的流程图，图1所示，如下步骤：
[0036](1)确定主手末端和从臂末端之间的映射关系。
[0037]具体地，根据DH坐标法对主手和从臂的机械臂分别进行运动学建模，得到各自末端相对于各自基座的位姿；本专利技术中，分别在主手和从臂的基座建立指向相同的基坐标系，进而可以得到主手末端和从臂末端之间的映射关系。
[0038](2)根据主手末端的实时位姿计算得到从臂各关节的期望位置，并据此进行控制。
[0039]在医生操作主手的末端时，控制器通过机器人正向运动学实时计算当前的主手的末端相对于其基座的位姿，并映射至从臂的末端，计算得到从臂的末端相对于其基座的期望位姿，其中，位置信息和姿态信息分别采用增量映射和绝对映射的方式；通过运动学逆解得到从臂各关节的期望位置并下发至其伺服电机，以达到从臂不断跟随医生操作的遥操作功能。
[0040](3)确定当前器械对应的从臂各关节的限位范围。
[0041]根据从臂的末端器械的类型，获取当前器械运动至其极限位置时从臂各关节的最大值和最小值，分别记为q
max
和q
min
；其中，器械上设有RFID设备以供读取该器械的参数信息，所述参数信息包括当前器械的类型及其极限位置；本专利技术设计一缓冲值e，据此得到从臂各关节运动的最大限位Q
max
和最小限位Q
min
，计算如下：
[0042]Q
max
＝q
max
‑
e
[0043]Q
min
＝q
min
+e...

【技术保护点】

【技术特征摘要】
1.一种遥操作的零力控制方法，其特征在于，包括：获取主手末端的实时位姿并映射至从臂末端，计算得到从臂各关节的期望位置，并控制从臂各关节运动；获取当前器械对应从臂各关节的限位范围，实时获取从臂各关节位置，判断其是否在所述限位范围内，并对不在所述限位范围内的关节进行阻抗控制，计算主手各关节的期望力矩并进行控制。2.根据权利要求1所述的遥操作的零力控制方法，其特征在于，所述阻抗控制具体为：构建阻抗控制器，得到对应关节的阻抗力τ为：τ＝K
p
*(q
m
‑
q
t
)+K
d
*(0
‑
v
t
)其中，K
p
、K
d
分别为阻抗控制器的刚度增益矩阵和阻尼矩阵；v
t
为该关节的当前速度，从该关节的电机编码器读取得到；q
m
为该关节的期望位置，q
t
为该关节的当前位置。3.根据权利要求2所述的遥操作的零力控制方法，其特征在于，K
p
＝mw2，K
d
＝2kmw；其中，m为主手的惯量，w为自然频率，k为阻尼比。4.根据权利要求2或3所述的遥操作的零力控制方法，其特征在于，所述关节的期望位置的获取具体为：若实时获取的关节位置大于其工作的限位范围的最大值，则设置其期望位置为该最大值；若实时获取的关节位置小于其工作的限位范围的最小值，则设置其期望位置为该最小值。5.根据权利要求1所述的遥操作的零力控制方法，其特征在于，所述计算主手各关节的期望力矩具体为：根据所述从臂各关节位置计算对应关节的雅克比矩阵，...

【专利技术属性】
技术研发人员：陈云川，桂凯，杨辉，
申请(专利权)人：佗道医疗科技有限公司，
类型：发明
国别省市：