一种主从式机器人的离合控制方法与流程

1.本发明涉及机器人技术领域，尤其涉及一种主从式机器人的离合控制方法。


背景技术：

2.目前微创手术已基本取代开放手术成为外科医学领域发展的主要方向，相比于传统的开放式手术，微创手术具有创伤小、病痛轻、恢复快等优势。随着机器人技术的发展，以达芬奇手术机器人为代表的基于机器人辅助系统的微创手术逐渐成熟，并被广泛应用。
3.手术机器人为主从遥操作式结构，医生通过操作主手来控制从手侧末端器械在内窥镜视野下运动，从而完成一系列复杂的手术操作。在主从式手术操作过程中，由于主从间位置映射存在设定的比例关系以及主刀医生操作的主观性，不可避免地会存在以下场景：(1)主操作手移动到其工作空间的边界位置，容易与控制台产生干涉；(2)主操作手间由于距离过近易产生碰撞；(3)主操作手虽然仍处在可达的工作空间内，但是位姿别扭，无法灵活移动等。
4.以上描述的场景被定义为“病态位姿”，对此，现有的解决方案是：在控制台上安装离合踏板，或在主操作手上安装离合开关，手术过程中，当主刀医生主观认为主操作手处于病态位姿时，踩下离合踏板或按下离合开关，主从连接即可中断，此时医生可拖动主操作手将其移动到舒适的位置，然后松开离合踏板或离合开关，主操作手会主动运动，以使得其姿态与当前受控器械姿态一致。此时主从重新完成匹配，手术可继续进行，以此往复。
5.现有的方案中，离合踏板或离合开关的安装无疑都增加了机械及电气安装的复杂性，手术过程中也存在较大的误触风险，致使手术的安全性降低。再者主从重新匹配时，主操作手需要主动运动，此时，主刀医生的手往往会被主操作手强行拖拽到目标姿态，极大地降低手术过程的连续性以及医生操作的舒适性，且延长了手术时间，增加手术风险。


技术实现要素：

6.发明目的：本发明针对上述不足，提出了一种主从式机器人的离合控制方法，降低了机械及电气安装的复杂性，降低了手术过程中的误触风险，同时提高了手术过程的连续性以及医生操作的舒适性。
7.技术方案：
8.一种主从式机器人的离合控制方法，包括步骤：
9.(1)根据主操作手和从臂器械的姿态可达空间划分主操作手的工作区和离合区；
10.(2)实时获取所述主操作手姿态，在所述主操作手离开所述工作区时断开主从连接，在所述主操作手进入所述工作区时匹配主从连接。
11.当操作者识别得到主操作手的位姿处于病态位姿时，拖动主操作手从工作区向所述离合区运动，并在所述主操作手进入所述离合区后调整所述主操作手的姿态。
12.所述步骤(1)中还设有过渡区，将从臂器械的姿态可达空间设为工作区；将主操作手的姿态可达空间内除从臂器械的姿态可达空间以外的区域中靠近工作区的部分区域设
为过渡区，远离工作区的部分区域设为离合区。
13.在所述主操作手从所述工作区进入所述过渡区时断开主从连接，并通过第一阻抗控制器规划出所述主操作手到达离合区的最短路径和目标姿态并沿着最短路径形成力矩通道；
14.在所述主操作手从所述离合区进入所述过渡区时，通过第二阻抗控制器规划出所述主操作手运动到离开工作区时的原始姿态的最短路径并沿着最短路径形成力矩通道。
15.所述第一阻抗控制器规划出所述主操作手到达离合区的最短路径和目标姿态具体为：
16.所述主操作手处于所述工作区与所述过渡区的边界时，记录当前所述主操作手各关节的位置；据此通过正向运动学计算得到当前主操作手的姿态矩阵r0；
17.根据当前的主操作手姿态矩阵r0的轴角形式计算得到所述离合区内距离当前的主操作手姿态矩阵r0最近的目标姿态矩阵r1对应的轴角形式，通过逆推计算得到目标姿态矩阵r1；
18.将当前的主操作手姿态矩阵r0和目标姿态矩阵r1分别表示为四元数形式，对四元数在当前姿态和目标姿态之间以主操作手末端运动形成的球面进行线性插值得到最短路径。
19.所述第二阻抗控制器规划出所述主操作手运动到离开工作区时的原始姿态的最短路径具体为：
20.所述主操作手处于所述离合区与所述过渡区的边界时，记录当前所述主操作手各关节的位置；据此通过正向运动学计算得到主操作手的当前姿态矩阵r2；
21.将主操作手的当前姿态矩阵r2和所述主操作手离开工作区时的原始姿态矩阵r0分别表示为四元数形式，对四元数在当前姿态和原始姿态之间以主操作手末端运动形成的球面进行线性插值得到最短路径。
22.操作者在所述过渡区拖动主操作手进入离合区或工作区的过程中若偏离对应的所述最短路径，则根据相应的阻抗控制器计算得到的对应的最短路径计算得到纠正力f＝k*sign(q
t-qa)*|q
t-qa|，使得主操作手沿着计算得到的最短路径运动；其中，k表示调参系数，qa表示t时刻偏离对应的最短路径后的当前位置，q
t
表示对应t时刻在对应的最短路径上的目标位置。
23.所述主操作手和从臂器械的姿态可达空间分别表示为mw和iw，则所述工作区为wz＝mw∩iw，所述过渡区为tz＝(mw iw)/2-wz，所述离合区为cz＝m
w-tz。
24.所述主操作手和从臂器械的姿态可达空间根据主操作手和从臂器械的各个姿态关节的关节限位通过正运动学计算获得。
25.有益效果：本发明无需增加离合踏板或离合开关，将主操作手的姿态可达空间划分为工作区和离合区，当主操作手到达病态位姿时，将主操作手向离合区拖动，自由调整姿态后再回到工作区；降低了机械及电气安装的复杂性，降低了手术过程中的误触风险，提高手术的安全性；且无需切换控制模式，整个过程以操作者为主导，提高了手术过程的连续性以及医生操作的舒适性。
附图说明
26.图1为主操作手末端位置点示意图。
27.图2为器械末端位置点示意图。
28.图3为本发明离合过程中的功能区域划分图以及路径规划图。
具体实施方式
29.下面结合附图和具体实施例，进一步阐明本发明。
30.图1为主操作手末端位置点示意图，如图1所示，主操作手的姿态关节四轴交于一点pm，定义从臂器械上存在一特征点ps与点pm对应，也即当主操作手的位置关节保持不动时，主操作手上的点pm保持不动，从臂器械上的点ps也保持不动；
31.本发明据此提供了一种主从式机器人的离合控制方法，包括如下步骤：
32.(1)根据主操作手各个姿态关节的关节限位通过正运动学可计算得到笛卡尔空间主操作手的姿态可达空间，记为mw；同理，根据从臂器械各个关节的关节限位通过正运动学可计算得到笛卡尔空间从臂器械的姿态可达空间，记为iw；并经过设置主操作手和从臂器械的关节限位，使得mw＞iw；
33.(2)根据步骤(1)得到的笛卡尔空间主操作手的姿态可达空间mw和笛卡尔空间从臂器械的姿态可达空间iw定义主操作手的工作区wz、过渡区tz和离合区cz，如图3所示；
34.其中，工作区wz＝mw∩iw，过渡区tz＝(mw iw)/2-wz，离合区cz＝m
w-tz；
35.(3)当操作者识别得到主操作手的位姿处于病态位姿时，拖动主操作手从工作区向离合区运动；
36.(4)主操作手内的主控制器实时获取主操作手姿态，在主操作手离开工作区时断开主从连接；在主操作手到达离合区后，操作者自由调整主操作手的位置，调整到位后将主操作手向工作区内拖动；
37.a.当主操作手姿态位于工作区wz内时，主控制器将主操作手姿态实时发送至从臂器械的从控制器，并由从控制器控制从臂器械跟随主操作手的位姿以完成相应的手术操作；
38.b.当主操作手姿态从工作区wz进入过渡区tz内时，主控制器记录当前的主操作手姿态矩阵r0，并中断与从臂器械的从控制器之间的信息交互，使得从臂器械各关节位置信息不再更新，从臂处于静止状态；此时，在主控制器上设置有第一阻抗控制器根据当前的主操作手姿态矩阵r0及其与离合区cz之间的最短距离得到主操作手的目标姿态矩阵r1，并据此规划主操作手的最短运动路径，并沿着最短路径形成力矩通道，以使得主操作手可在操作者操控下通过零力控制以最短路径通过过渡区tz；
39.其中，第一阻抗控制器根据当前的主操作手姿态矩阵r0及其与离合区cz之间的最短距离得到主操作手的目标姿态矩阵r1具体为：
40.主操作手处于工作区wz与过渡区tz的边界时，记录当前主操作手各关节的位置[θ1,θ2,
…
,θn]，其中，n表示主操作手的关节数量；据此通过正向运动学计算得到当前主操作手的姿态矩阵r0，如图3所示，并以轴角形式表示[op0,α0]，其中，op0表示当前主操作手的姿态可达空间顶点到当前末端位置p0的方向向量，α0表示主操作手初始姿态与当前姿态r0之间的夹角，所述主操作手初始位姿为当器械的腕关节以及开合关节处于关节零位(即器
械处于零位)时对应的主操作手的位姿；
[0041]
将离合区cz内距离当前的主操作手末端位置p0最近的点p1作为其目标位置，根据当前的主操作手姿态矩阵r0的轴角形式计算得到主操作手的目标姿态矩阵r1对应的轴角形式[op1,α1]，则通过逆推计算得到目标姿态矩阵r1；
[0042]
将当前的主操作手姿态矩阵r0和目标姿态矩阵r1分别表示为四元数形式q0和q1，对四元数进行在点p0和点p1之间以主操作手末端运动形成的球面上进行线性插值即可获得当前主操作手末端位置p0至其目标位置p1的最短路径为q
t
＝slerp(q0,q1,t)，其中，t表示最短路径的运动时刻序列，q
t
表示当前的主操作手姿态矩阵r0至其目标姿态矩阵r1的最短路径上对应运动时刻序列的主操作手末端位置集合；
[0043]
c.当主操作手姿态处于离合区cz时，主控制器提供补偿力矩使得主操作手可在操作者操控下在离合区cz内以零力控制自由拖动，当操作者将主操作手移动到主观舒适的位置时，停止拖动主操作手，主控制器停止提供补偿力矩；
[0044]
d.当主操作手姿态从离合区cz进入过渡区tz内时，主控制器记录当前的主手末端姿态矩阵r2，此时，以工作区wz内主操作手的原始末端位置p0作为其目标位置，在主控制器上的第二阻抗控制器根据当前的主操作手姿态矩阵r2，规划出主操作手运动到工作区wz内原始姿态矩阵r0的最短运动路径，并沿着最短路径形成力矩通道，以使得主操作手可在操作者操控下通过零力控制以最短路径通过过渡区tz，主操作手末端到达工作区wz时的实际姿态矩阵为r0'；若r0与r0'之间的误差err在预设范围内，则认为主操作手姿态调整到位；
[0045]
第二阻抗控制器根据当前的主操作手姿态矩阵r2，规划出主操作手运动到工作区wz内原始姿态矩阵r0的最短运动路径具体为：
[0046]
主操作手处于离合区cz与过渡区tz的边界时，记录当前主操作手各关节的位置[θ1,θ2,
…
,θn]，其中，n表示主操作手的关节数量；据此通过正向运动学计算得到当前主操作手的姿态矩阵r2，
[0047]
将工作区wz内原始姿态矩阵r0和当前的主操作手姿态矩阵r2分别表示为四元数形式q0和q2，对四元数在当前主操作手末端位置p2和原始位置p0之间以主操作手末端运动形成的球面上进行线性插值即可获得当前主操作手末端位置p2至其原始位置p0的最短路径为q
t
'＝slerp(q0,q2,t)，q
t
'表示当前主操作手末端位置p2至其原始位置p0的最短路径上对应运动时刻序列的主操作手末端位置集合；
[0048]
本步骤中，进一步地，若操作者在过渡区tz拖动主操作手进入离合区cz或工作区wz的过程中若偏离前述计算得到的对应的最短路径，则控制器根据前述阻抗控制器计算得到的对应的最短路径提供额外的纠正力f＝k*sign(q
t-qa)*|q
t-qa|，其中，qa表示t时刻偏离对应的最短路径后的当前位置，q
t
表示对应t时刻在对应的最短路径上的目标位置；使得主操作手沿着计算得到的对应的最短路径运动，也即迫使操作者沿对应的最短路径拖动主操作手；其中，k表示调参系数；
[0049]
(5)主操作手进入工作区wz后，从臂器械的从控制器开启，将主操作手的位置信息和姿态信息分别以增量映射方式和绝对映射方式传递给从臂器械的从控制器，从臂器械据此跟随主操作手的位姿变化完成相应的手术操作。
[0050]
本发明无需增加离合踏板或离合开关，将主操作手的姿态可达空间划分为工作区和离合区，当主操作手到达病态位姿时，将主操作手向离合区拖动，自由调整姿态后再回到
工作区；降低了机械及电气安装的复杂性，降低了手术过程中的误触风险，提高手术的安全性；且无需切换控制模式，整个过程以操作者为主导，提高了手术过程的连续性以及医生操作的舒适性。
[0051]
以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。