CN117770968A - 手术机器人的机械臂控制方法及控制装置、电子设备 - Google Patents

Abstract

本发明涉及一种手术机器人的机械臂控制方法、手术机器人的机械臂控制装置、电子设备及计算机可读取介质。其中，手术机器人的机械臂控制方法包括：基于机械臂末端执行器的位姿信息与目标点，确定偏差，其中，目标点为根据手术切口确定的末端执行器的远端运动中心；若偏差大于或等于第二阈值，则基于偏差确定机械臂的停止方案；基于停止方案，停止机械臂。通过获取手术机器人机械臂末端执行器的位姿，确定机械臂末端执行器相对于远端运动中心的偏移，在偏移较大时，根据偏移情况确定不同的停止方案，通过不同的停止方案停止末端执行器，防止末端执行器偏移导致的对病人创口的拉扯，提高了手术的安全性。

Description

手术机器人的机械臂控制方法及控制装置、电子设备

技术领域

本发明涉及机器人技术领域，特别涉及一种手术机器人的机械臂控制方法、手术机器人的机械臂控制装置、电子设备及计算机可读取介质。

背景技术

腹腔镜手术机器人是一种在手术前，腹腔镜微创手术需要在患者身上切开几个小的切口，并将套管放置于切口内作为手术器械通道，将末端执行器在机械臂摆位操作下通过病人腹腔上的手术孔进入病患体内。切口的存在使得器械只能沿器械轴线进行平移以及沿器械轴上的瞬时不动点进行旋转，这个瞬时不动点称为远端运动中心(RCM，RemoteCenter of Motion)。机械臂在运动的同时，其末端器械运动始终需要围绕远端运动中心，机械臂末端执行器相对于远端运动中心容易产生偏移，对病人的创口产生拉扯力，危及病人生命安全，安全性低。

发明内容

为克服相关技术中存在的问题，本公开的示例性实施例提供了一种手术机器人的机械臂控制方法、手术机器人的机械臂控制装置、电子设备及计算机可读取介质。

本公开的第一方面的示例性实施例提供了一种手术机器人的机械臂控制方法，其中，机械臂控制方法包括：基于机械臂末端执行器的位姿信息与目标点，确定偏差，其中，目标点为根据手术切口确定的末端执行器的远端运动中心；若偏差大于或等于第一阈值，则直接停止机械臂；若偏差大于或等于第二阈值，且小于第一阈值，则将末端执行器停至目标点，其中，第二阈值小于第一阈值。

在一些实施例中，基于机械臂末端执行器的位姿信息与目标点，确定偏差，包括：获取末端执行器的当前位姿，及当前位姿之前的一个或多个时刻的历史位姿；分别确定当前位姿及一个或多个时刻的历史位姿与目标点之间的偏移量，其中，偏移量为目标点到任一时刻的末端执行器轴线的距离；基于全部偏移量，确定偏差。

在一些实施例中，获取末端执行器的当前位姿，及当前位姿之前的一个或多个时刻的历史位姿，包括：获取末端执行器当前时刻的位姿信息，作为当前位姿；将当前时刻之前n个时刻的位姿信息作为历史位姿，其中，n为整数且10≤n≤30。

在一些实施例中，基于全部偏移量，确定偏差，包括：基于全部偏移量计算加权平均值，确定偏差，其中，当前位姿对应的偏移量的权重，大于与当前位姿时间间隔最远的历史位姿对应的偏移量的权重。

在一些实施例中，第二阈值大于或等于3mm，且小于或等于7mm；第一阈值大于或等于8mm，且小于或等于12mm。

在一些实施例中，将末端执行器停至目标点，包括：根据末端执行器的当前位姿以及目标点，确定停止路径；基于停止路径控制机械臂，使末端执行器停至目标点。

在一些实施例中，在执行根据末端执行器的当前位姿以及目标点，确定停止路径步骤之前或同时，将末端执行器停至目标点，还包括：控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动。

在一些实施例中，控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动，移动距离大于或等于1cm。

在一些实施例中，手术机器人的机械臂控制方法还包括：响应于外部停止信号，停止机械臂。

第二方面，根据另一些示例性实施例，本公开还提供了一种手术机器人的机械臂控制装置，其中，机械臂控制装置包括：偏差确定模块，用于基于机械臂末端执行器的位姿信息与目标点，确定偏差，其中，目标点为根据手术切口确定的末端执行器的远端运动中心；执行模块，在偏差大于或等于第一阈值情况下，用于直接停止机械臂；在偏差大于或等于第二阈值，且小于第一阈值情况下，用于将末端执行器停至目标点，其中，第二阈值小于第一阈值。

第三方面，根据另一些示例性实施例，本公开还提供了一种电子设备，其中，电子设备包括：存储器、处理器及存储在存储器上并可在处理器上运行的数据处理程序，数据处理程序被处理器执行时实现如第一方面所述的手术机器人的机械臂控制方法。

第四方面，根据另一些示例性实施例，本公开还提供了一种计算机可读取介质，该计算机可读取介质存储有如下程序，程序用于执行如第一方面所述的手术机器人的机械臂控制方法。

本公开的实施例提供的技术方案可以包括以下有益效果：通过获取手术机器人机械臂末端执行器的位姿，确定机械臂末端执行器相对于远端运动中心的偏移，在偏移较大时，根据偏移情况确定不同的停止方案，通过不同的停止方案停止末端执行器，防止末端执行器偏移导致的对病人创口的拉扯，提高了手术的安全性。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。

附图说明

通过结合附图对于本发明的示例性实施例进行描述，可以更好地理解本发明，在附图中：

图1是根据一本公开示例性实施例示出的手术机器人的机械臂控制方法流程示意图；

图2是根据一本公开示例性实施例示出的手术机器人末端执行器与RCM偏差的示意图；

图3是根据另一本公开示例性实施例示出的手术机器人的机械臂控制方法流程示意图；

图4是根据另一本公开示例性实施例示出的手术机器人的机械臂控制方法流程示意图；

图5是根据另一本公开示例性实施例示出的手术机器人的机械臂控制方法流程示意图；

图6是根据另一本公开示例性实施例示出的手术机器人的机械臂控制方法流程示意图；

图7是根据另一本公开示例性实施例示出的手术机器人的机械臂控制方法流程示意图；

图8是根据另一本公开示例性实施例示出的手术机器人的机械臂控制方法流程示意图；

图9是根据一本公开示例性实施例示出的手术机器人末端执行器停止路径规划的示意图；

图10是根据另一本公开示例性实施例示出的手术机器人末端执行器停止路径规划的示意图；

图11是根据一本公开示例性实施例示出的一种手术机器人的机械臂控制装置的示意图；

图12是根据一本公开示例性实施例示出的一种电子设备的示意框图。

具体实施方式

以下将描述本发明的具体实施方式，需要指出的是，在这些实施方式的具体描述过程中，为了进行简明扼要的描述，本说明书不可能对实际的实施方式的所有特征均作详尽的描述。应当可以理解的是，在任意一种实施方式的实际实施过程中，正如在任意一个工程项目或者设计项目的过程中，为了实现开发者的具体目标，为了满足系统相关的或者商业相关的限制，常常会做出各种各样的具体决策，而这也会从一种实施方式到另一种实施方式之间发生改变。此外，还可以理解的是，虽然这种开发过程中所作出的努力可能是复杂并且冗长的，然而对于与本发明公开的内容相关的本领域的普通技术人员而言，在本公开揭露的技术内容的基础上进行的一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本公开的内容不充分。

除非另作定义，权利要求书和说明书中使用的技术术语或者科学术语应当为本发明所属技术领域内具有一般技能的人士所理解的通常意义。本发明专利申请说明书以及权利要求书中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。“一个”或者“一”等类似词语并不表示数量限制，而是表示存在至少一个。“包括”或者“包含”等类似的词语意指出现在“包括”或者“包含”前面的元件或者物件涵盖出现在“包括”或者“包含”后面列举的元件或者物件及其等同元件，并不排除其他元件或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，也不限于是直接的还是间接的连接。

为了满足RCM的限制，在一些相关技术中，通常有机械RCM机构(双平行四边形机构)、被动关节，可通过机械结构控制RCM点的精度，但其精度好坏取决于设计制造以及工艺水平，且精度后期很难调整。由于机器人实际上是存在一定误差的。机器人所执行的结果和期望指令并不能完全一致。本公开提供的方案具有通用性和可扩展性，且不需要特定机构，用常规7自由度机械臂去实现，通过算法增加RCM的约束控制。通过本公开实施例公开的软件停止策略实现机器人在手术系统中绕空间不动点的运动控制的同时，可有效保证手术过程中病人的安全性。

为解决上述技术问题，本公开的第一方面的示例性实施例提供了一种手术机器人的机械臂控制方法100，其中，手术机器人可以包括机械臂、安装于机械臂法兰的转接机构或者夹持机构、与转接机构或者夹持机构连接的末端执行器。在一些实施例中，手术机器人可以是腹腔镜手术机器人。机械臂可以是7自由度，机械臂可以为包含至少6个以上关节的串联式机械臂，转接机构或者夹持机构可以选择和机械臂第七轴同轴也可以选择不同轴，能够实现末端执行器多方位多角度的移动。

如图1所示，手术机器人的机械臂控制方法100可以包括：步骤S110、步骤S120以及步骤S130，以下进行详细描述。

步骤S110，基于机械臂末端执行器的位姿信息与目标点，确定偏差，其中，目标点为根据手术切口确定的末端执行器的远端运动中心。

末端执行器的位姿信息是末端执行器实际的位置和姿态信息，位姿信息可以通过机械臂的基坐标系表达。目标点为根据手术切口确定的远端运动中心，即RCM，例如，腹腔镜手术机器人在手术前，在患者身上切开几个小的切口，并将套管(Trocar)放置于切口内作为手术器械通道，将末端执行器在机械臂摆位操作下通过病人腹腔上的手术孔进入病患体内。切口的存在使得器械只能沿器械轴线进行平移以及沿器械轴上的瞬时不动点进行旋转，这个瞬时不动点称为RCM。理想状态下，末端执行器的实际不动点应与RCM重合，换句话说RCM位于末端执行器的轴线上。而在实际手术过程中，机器人为奇异位形或者机械臂位于极限位形等情况下，末端执行器的实际位置会与RCM有一定的偏离，如果是微小的偏差则基本不会对手术造成实质影响，而如果偏差较大则会影响手术安全性，因此，需要检测末端执行器的实际位姿并与理想的RCM比较以确定偏差。偏差可以是基于RCM和末端执行器某一点或轴线之间的实际距离确定的。

在一些实施例中，偏差可以为目标点，即RCM，到末端执行器轴线的距离。末端执行器整体呈细长针状，其长度的延伸方向为轴线方向。如图2所示，当RCM与末端执行器轴线有偏差时，可以通过如下公式确定RCM到末端执行器轴线的偏差：

其中，p_rcm为实际RCM点的位置，p_f为机械臂的法兰中心点位置，以p_f为原点建立机械臂法兰坐标系，z轴沿末端执行器轴线，向末端执行器的工作端延伸，机械臂法兰坐标系xy平面与法兰盘共平面，p′_rcm为p_rcm在机械臂法兰坐标系xy平面上的投影。向量的长度为RCM到末端执行器轴线的偏差，其物理含义表示当前器械轴沿平行方向到RCM点的距离。末端执行器轴线的朝向与法兰端朝向相同。末端执行器朝向R＝[x_f,y_f,z_f]，其中R、p_rcm、p_f都是相对于机器人基坐标系下的描述，/>相对于法兰坐标系下的描述，用d_err表示当前时刻RCM的偏差。根据偏差，执行后续步骤：

若偏差大于或等于第一阈值，则执行：步骤S120，直接停止机械臂。

若偏差大于或等于第二阈值，且小于第一阈值，则执行：步骤S130，将末端执行器停至目标点，其中，第二阈值小于第一阈值。

本公开实施例中，如果检测得到的末端执行器的实际位姿与RCM之间的偏差较小，即小于第二阈值的情况下，可认为偏差不会影响手术正常进行，不会造成安全问题，可不进行调整或停止。如果偏差较大，则需要停止机械臂，从而避免安全风险。并且，本公开能够根据偏差的实际情况，即根据偏差的大小，确定不同的停止方案，从而基于不同的停止方案，停止机械臂，以避免末端执行器拉扯伤口，一方面保证手术的安全性，另一方面保证手术效率。

在偏差大于或等于第一阈值的情况下，偏差程度较为严重，已经影响手术安全性。因此步骤S120直接停止机械臂，即将末端执行器停止在当前的实际位姿，避免进一步的偏离移动造成的更严重的危险。在停止后，可通过人工手段将末端执行器移动至RCM，并检测手术机器人是否存在故障，检测无误后可继续手术。

在一些实施例中，步骤S120直接停止机械臂，可以通过向机器人下发当前关节角度命令值完成。在偏差大于或等于第一阈值时，机器人检测当前末端执行器的位姿以及机器人各关节的角度，同时将机器人当前末端执行器的位姿以及机器人各关节的角度信息下发给机器人，使机器人保持在当前位置，强制停止机械臂。

在偏差大于或等于第二阈值，且小于第一阈值的情况下，可以认为存在安全隐患，需要停止手术并调整末端执行器的位姿状态。可以通过步骤S130，将末端执行器移动至RCM的位置，并停止，确认手术机器人状态后，可以在RCM位于末端执行器轴线的状态下继续进行手术，从而避免了在偏离状态下手术的危险性。

如果根据偏差，在偏差大于一定程度时，将末端执行器停至目标点作为唯一停止方案，则在手术过程中，在末端执行器偏差过大的情况下，末端执行器在向目标点移动的过程中，由于移动的路径可能会进一步拉扯伤口，可能会对人体组织造成更严重的伤害。另一方面，如果根据偏差，在偏差大于一定程度时，将直接停止作为唯一的停止方案，则在手术过程中，可能会导致多次急停，且在停止后需要对机械臂末端执行器进行手动调整，耗费人力、降低手术效率，影响手术过程。本公开实施例中，能够根据偏差的大小，确定不同的停止方案，包括直接停止机械臂，以及将末端执行器停至目标点；这种基于不同的停止方案，停止机械臂的控制方法，能够有效地保证手术的效率与安全性，实用性更强。

在一些实施例中，步骤S130可以基于末端执行器的当前实际位姿进行路径规划，使末端执行器能够平滑、稳定的移动到RCM并停止。由于末端执行器当前时刻位置存在一定偏差，并且可能具有速度、加速度，规划的路径中可能存在相比于当前位置与RCM偏差更大的点，因此在偏差已经较大的情况下(例如偏差大于或等于第一阈值的情况下)，需要立刻停止末端执行器的移动，通过人工手段将末端执行器移动至RCM点，避免进一步拉扯伤口，造成更严重的后果，所以在步骤S120中，需要直接停止机械臂。在偏差较小时(例如偏差大于或等于第二阈值且小于第一阈值的情况下)，即使规划的路径中存在相比于当前位置与RCM偏差更大的点，仍在伤口及皮肤所能承受的拉扯范围内，末端执行器仍不会对伤口产生危害，因此步骤S130中，可以基于末端执行器的当前实际位姿进行路径规划，使末端执行器平滑、稳定地移动到RCM并停止；在末端执行器移动到RCM点并停止后，可以根据机器人状况及手术需求，启动机器人继续进行手术，从而有效节省了手动调整末端执行器的时间，提高了手术的效率。

在一些实施例中，第二阈值可以大于或等于3mm，且小于或等于7mm；第一阈值可以大于或等于8mm，且小于或等于12mm。在末端执行器相对于RCM产生偏移时，根据手术创口的尺寸可以确定第二阈值，第二阈值可以大于或等于3mm，且小于或等于7mm，第二阈值可以是用于判断末端执行器的偏差是否存在手术安全隐患的极限值，当末端执行器的偏差小于第二阈值时，可以认为偏差不会影响手术正常进行，不会造成安全问题，可不进行调整或停止；当末端执行器的偏差大于或等于第二阈值时，视为具有手术风险，需要对末端执行器进行调整和停止，偏差小于3mm时不会影响手术安全，若第二阈值设置过小，小于3mm时，机器人频繁停止，影响手术效率；偏差大于7mm时已经能够影响手术安全，若第二阈值设置过大，大于7mm时，则无法起到降低风险的作用，因此第二阈值可以大于或等于3mm，且小于或等于7mm。第二阈值可以是5mm，在偏差小于5mm时，可以认为偏差对手术不造成影响，能够继续进行手术，无需进行调整；根据手术创口的尺寸以及皮肤所能承受的拉扯力可以确定第一阈值，第一阈值可以大于或等于8mm，且小于或等于12mm；第一阈值可以是用于判断末端执行器的偏差是否严重影响手术安全的极限值，当末端执行器的偏差大于或等于第一阈值时，偏差程度较为严重，会严重影响手术安全性，需要直接停止机械臂，将末端执行器停止在当前的实际位姿，避免进一步的偏离移动造成的更严重的危险。当末端执行器的偏差大于等于第二阈值且小于第一阈值时，可以认为存在安全隐患，需要停止手术并调整末端执行器的位姿状态；若设置第一阈值小于8mm，则可能导致机器人频繁停止，且需要人工调整，影响手术效率；若第一阈值大于12mm，则末端执行器已经对伤口产生拉扯和伤害，无法保证安全性，因此第一阈值可以大于或等于8mm，且小于或等于12mm。第一阈值可以是10mm，当偏差大于或等于10mm时，认为末端执行器当前偏差过大，即将超过皮肤所能承受的拉扯力，对手术有影响，立刻停止末端执行器的移动，避免进一步拉扯伤口。当偏差大于或等于5mm且小于10mm时，可以认为末端执行器后续动作对手术创口可能产生拉扯，因此停止手术并调整末端执行器的位姿状态将末端执行器移动至RCM的位置。第一阈值和第二阈值的设定可以防止手术中末端执行器对伤口造成的拉扯与撕裂，保证手术安全。

本公开通过获取手术机器人机械臂末端执行器的位姿，确定机械臂末端执行器相对于RCM的偏移，在偏差较小的情况下，无需调整；在偏差超过一定范围的情况下，可以通过规划路径将末端执行器调整至RCM，从而避免安全风险，及时调整末端执行器的位姿；而在偏差较大的情况下，需要立刻直接停止机械臂，通过人工手段将末端执行器移动至RCM，并在检测手术机器人无误后继续手术。本公开根据偏移情况确定不同的停止方案，通过不同的停止方案停止末端执行器，防止末端执行器偏移导致的对病人创口的拉扯，提高了手术的安全性。

在一些实施例中，如图3所示，步骤S110，基于机械臂末端执行器的位姿信息与目标点，确定偏差，可以包括：步骤S111，获取末端执行器的当前位姿，及当前位姿之前的一个或多个时刻的历史位姿；步骤S112，分别确定当前位姿及一个或多个时刻的历史位姿与目标点之间的偏移量，其中，偏移量为目标点到任一时刻的末端执行器轴线的距离；步骤S113，基于全部偏移量，确定偏差。机械臂末端执行器的位姿信息可以包括末端执行器当前时刻的位姿，以及当前时刻之前的一个或多个时刻的历史位姿，获取位姿信息，在本公开实施例中，手术机器人可以周期性的持续检测机械臂的位置和姿态，当前位姿可以是当前时刻或当前检测周期的末端执行器位置、姿态，可以通过当前时刻机械臂各关节的关节转角进行表示，也可以通过当前时刻末端执行器在机械臂的基坐标系上的位置坐标进行表示；历史位姿可以是存储于机器人存储单元中的，当前时刻之前一个或多个时刻，或当前检测周期之前的一个或多个检测周期的末端执行器的位置、姿态，可以通过当前时刻之前一个或多个时刻的机械臂各关节的关节转角进行表示，也可以通过当前时刻之前一个或多个时刻的末端执行器在机械臂的基坐标系上的位置坐标进行表示。历史位姿的获取周期可以根据手术机器人机械臂控制方法的控制周期确定，机械臂控制周期较小，机械臂的控制周期可以是1ms，在获取多个时刻的位姿信息时，采样间隔时间短，偏移量为RCM到末端执行器的轴线的距离，确定当前时刻的位姿以及一个或多个时刻的历史位姿的偏移量，获取每个时刻的末端执行器轴线与RCM点之间的距离，即获取每个时刻的偏移量，根据多个偏移量，确定偏差，偏差可以基于多个偏移量进行计算获取，偏差可以通过计算多个偏移量的平均值进行确定。在通过单点检测的方式获取偏差时，由于机器人在控制过程中可能存在瞬时扰动，也可能存在检测误差，导致当前时刻的偏差较大，但由于瞬时扰动而产生末端执行器偏离不会长时间保持，末端执行器能够立刻回正，同时，由于机械臂的控制周期较短，在连续获取机械臂的位姿信息时，采样时间间隔短，仅仅获取当前时刻的末端执行器位姿，会导致瞬时扰动状态下的测量误差影响偏差的测量结果，可能导致机械臂在根据偏差进行急停，机械臂工作连续性差，手术安全性差，通过这种方法，能够考虑到机械臂瞬时扰动导致的测量误差，并且可以将这种测量误差进行平均，有效减小获取偏差时的误差，能够获取更加准确的偏差，提高系统的精度，提高算法的可靠性。

在一些实施例中，如图4所示，步骤S111，获取末端执行器的当前位姿，及当前位姿之前的一个或多个时刻的历史位姿，包括：步骤S1111，获取末端执行器当前时刻的位姿信息，作为当前位姿；步骤S1112，将当前时刻之前n个时刻的位姿信息作为历史位姿，其中，n为整数且10≤n≤30。当前位姿可以是末端执行器当前时刻的位姿信息，历史位姿可以是当前时刻之前n个时刻的位姿信息，n可以是整数，且10≤n≤30，即需要获取大于等于10且小于等于30个时刻的历史位姿，当n小于10时，在机械臂受到瞬时扰动导致测量误差时，由于获取位姿信息的采样数量过少，可能导致确定偏差时，瞬时扰动导致的测量误差对偏差的确认造成较大影响，从而造成误判导致手术机器人错误停止；当n大于30时，获取位姿信息的采样数量过多，可能导致实际的末端执行器当前已经存在较大偏移量的情况下，确定的偏差仍然较小，不能及时的停止手术机器人导致产生手术风险。历史位姿的获取周期可以根据手术机器人机械臂控制方法的控制周期确定，机械臂控制周期较小，机械臂的控制周期可以是1ms，在获取多个时刻的位姿信息时，采样间隔时间短。n可以为20，历史位姿可以是当前时刻之前20个时刻的位姿信息，当n为20时，能够有效减少采样过程中瞬时扰动导致的测量误差，且保证检测的准确性和可靠性。因此获取当前时刻之前n个时刻的位姿信息，能够有效防止控制过程中的瞬时扰动对偏差的确定带来的影响。

在一些实施例中，步骤S113，基于全部偏移量，确定偏差，可以包括：基于全部偏移量计算加权平均值，确定偏差，其中，当前位姿对应的偏移量的权重，大于与当前位姿时间间隔最远的历史位姿对应的偏移量的权重。基于当前时刻以及当前时刻之前一个或多个时刻的末端执行器偏移量，进行加权平均值计算，当前时刻对应的末端执行器偏移量权重较大，距离当前时刻时间间隔最远的时刻所对应的末端执行器偏移量权重小于当前时刻对应的末端执行器偏移量权重，即，越靠近当前时刻，权重越大。加权平均值计算时，多个时刻的末端执行器偏移量权重系数可以由当前时刻向历史时刻逐渐减小，当前时刻权重系数最大，距离当前时刻最远的历史时刻权重系数最小；也可以将多个时刻的末端执行器偏移量分段，当前时刻以及靠近当前时刻的一个或多个时刻所对应的权重系数最大，距离当前时刻时间间隔最远的时刻所对应的权重系数最小，中间的一个或多个时刻所对应的权重系数大于当前时刻所对应的权重系数，且小于距离当前时刻时间间隔最远的时刻所对应的权重系数。通过加权平均计算偏差，能够有效避免机械臂扰动的影响，获取更加准确的机械臂末端执行器相对于RCM的偏差。末端执行器的偏移量权重系数的确定时，当前时刻权重系数最大，距离当前时刻最远的历史时刻权重系数最小，使当前时刻的偏移量在确定偏差时影响更大，距离当前时刻最远的历史时刻权重系数最小，使距离当前时刻越远的历史时刻的偏移量在确定偏差时影响更小，即在确定偏差时，更加注意当前的偏移量，能够使得获取的机械臂末端执行器的偏差更加准确，判断是否停止的依据更加可靠。例如，取20个时刻对应的末端执行器偏移量进行加权平均计算确定偏差，加权平均值误差可以记为公式如下：

其中，d_err(i)为第i个时刻所对应的末端执行器相对于RCM的偏移量，a(i)表示第i个时刻所对应的权重系数。其中，20个时刻对应的权重系数可以为：

a＝[0.9,0.9,0.9,0.9,0.9,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.1,1.1,1.1,1.1,1.1]

其中，最靠近当前时刻5个时刻权重系数取1.1，中间10个时刻的权重系数取1.0，距离当前时刻时间间隔最远的5个时刻的权重系数取0.9，即，越靠近当前时刻，权重系数越大。

在一些实施例中，如图5所示，步骤S130，将末端执行器停至目标点，可以包括：步骤S131，根据末端执行器的当前位姿以及目标点，确定停止路径；步骤S132，基于停止路径控制机械臂，使末端执行器停至目标点。当偏差大于或等于第二阈值，且小于第一阈值时，根据末端执行器当前的位姿，确定末端执行器轴线上一点为起始点位置，并以RCM为目标点位置，根据起始点位置、目标点位置规划停止路径，所规划的停止路径可以在末端执行器姿态不变的情况下，使末端执行器轴线上的起始点能够平移至与目标点位置重合。根据规划的停止路径，控制机械臂运动，使末端执行器移动至目标点并停止。由于末端执行器在停止过程中姿态保持不变，进行路径规划时可以根据起始点位置和目标点位置进行轨迹插补，获取停止过程中的目标位姿，从而求得机器人的指令，便于进行控制。通过保持末端执行器姿态不变的路径规划方法，能够有效避免停止过程中，末端执行器姿态改变导致的拉扯伤口的风险。轨迹插补可以通过S型加减速插补算法进行在线插补规划，能够生成最优的平滑运动轨迹，有较高的性能。可以使用S型加减速插补算法速度插补模式进行路径规划，获取当前末端执行器起始点位置p_e、起始速度v_e、起始加速度a_e并输入插补算法模块，设定目标速度v^′ _e、目标加速度a^＇ _e均为零，使机器人停止，根据末端执行器当前位姿信息以及手术情况，确定最大速度V_max、最大加速度A_max，将最大角加速度J_max作为插补条件输入插补算法模块中，S型加减速插补算法即可对机器人末端执行器进行路径和速度的规划，随后根据规划的路径控制机械臂末端执行器的停止。

在一些实施例中，如图6、图7所示，在执行根据末端执行器的当前位姿以及目标点，确定停止路径步骤之前或同时，步骤S130，将末端执行器停至目标点，还可以包括：步骤S133，控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动。如图6所示，在进行末端执行器停止路径规划之前，可以首先控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动，使末端执行器从手术空间中退出一定距离，从而保证末端执行器离开人体组织，防止在后续根据路径规划移动时，末端执行器的移动对人体组织造成伤害。如图7所示，也可以将末端执行器沿轴线向末端执行器与机械臂的连接端方向移动，与停至RCM的移动路径同步完成，可以将末端执行器沿轴线向末端执行器与机械臂的连接端方向移动纳入停止路径的规划中，能够在停止路径规划时避免末端执行器与人体组织接触，避免末端执行器移动导致对手术创口的拉扯和划伤，保证手术的安全性。

例如：首先通过计算获得末端执行器的目标点位置：

其中，如图9所示，p_e为起始点，起始点姿态为R_e，R_e＝[x_f,y_f,z_f]，p^′ _e为目标点；^fp_e为法兰坐标系下的器械末端位置， ^fp_rcm为RCM点在法兰坐标系下的投影， ^fp_e|x、^fp_e|y、^fp_rcm|z分别表示^fp_e的xy分量和^fp_rcm的z分量。

对⁰p′_e的位置进行求解，⁰p′_e表示p′_e相对于机械臂基坐标系{0}的位置：

其中：d_z为末端执行器沿轴线向末端执行器与机械臂的连接端方向移动的距离，为p_m沿法兰坐标系z轴负方向移动d_z后所得的向量。最终得到目标点p＇_e位置，随后可以根据起始点位置以及目标点位置，进行停止路径规划。

在一些实施例中，控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动，移动距离大于或等于1cm。由于手术中末端执行器接触人体组织，在控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动时，设置移动距离大于或等于1cm，能够确保末端执行器远离人体组织，避免末端执行器在根据路径停止时对人体组织的拉扯与划伤，保证手术的安全性。在一些实施例中，退出的距离可以为2cm，从而确保末端执行器在移动过程中不会划伤人体组织，并且该距离可以根据手术情况、手术对象进行调整，以确保手术的安全。

在一些实施例中，如图8所示，手术机器人的机械臂控制方法100还可以包括：步骤S140，响应于外部停止信号，停止机械臂。手术机器人的机械臂可以在RCM未产生偏移的情况下根据外部停止信号实现正常的停止动作，当获取到外部停止信号，手术机器人进行响应，执行停止动作。由于末端执行器存在速度，可以对末端执行器的速度进行规划，从而实现对机械臂末端执行器的安全停止，达到停止过程平滑的效果。响应于外部停止信号的停止路径规划，可以使用S型加减速插补算法进行速度插补。获取起始位置为p_e、起始速度v_e、起始加速度a_e；并在插补算法模块中设定目标速度v′_e、目标加速度a′_e均为零，使让机器人停止。根据末端执行器当前位姿信息以及手术情况，确定最大速度V_max、最大加速度A_max，将最大角加速度J_max作为插补条件输入插补算法模块中，插补算法模块即可对机器人末端执行器进行路径和速度的规划，随后根据规划的路径控制机械臂末端执行器的停止。如图10所示，在响应于外部停止信号，停止机械臂的过程中，停止路径受RCM约束控制，能够实现对机械臂末端执行器的安全停止，达到停止过程平滑的效果，同时避免停止过程中拉扯伤口，保证手术停止过程的安全性。

基于同一发明构思，如图11所示，本公开还提供了一种手术机器人的机械臂控制装置200，其中，机械臂控制装置200包括：偏差确定模块210，用于基于机械臂末端执行器的位姿信息与目标点，确定偏差，其中，目标点为根据手术切口确定的末端执行器的远端运动中心；执行模块220，在偏差大于或等于第一阈值情况下，用于直接停止机械臂；在偏差大于或等于第二阈值，且小于第一阈值情况下，用于将末端执行器停至目标点，其中，第二阈值小于第一阈值。

在一些实施例中，偏差确定模块210，可以包括：获取单元，用于获取末端执行器的当前位姿，及当前位姿之前的一个或多个时刻的历史位姿；偏移量确定单元，用于分别确定当前位姿及一个或多个时刻的历史位姿与目标点之间的偏移量，其中，偏移量为目标点到任一时刻的末端执行器轴线的距离；计算单元，用于基于全部偏移量，确定偏差。

在一些实施例中，获取单元通过以下方式获取末端执行器的当前位姿，及当前位姿之前的一个或多个时刻的历史位姿：获取末端执行器当前时刻的位姿信息，作为当前位姿；将当前时刻之前n个时刻的位姿信息作为历史位姿，其中，n为整数且10≤n≤30。

在一些实施例中，计算单元通过以下方式基于全部偏移量，确定偏差：基于全部偏移量计算加权平均值，确定偏差，其中，当前位姿对应的偏移量的权重，大于与当前位姿时间间隔最远的历史位姿对应的偏移量的权重。

在一些实施例中，执行模块220通过以下方式将末端执行器停至目标点：根据末端执行器的当前位姿以及目标点，确定停止路径；基于停止路径控制机械臂，使末端执行器停至目标点。

在一些实施例中，执行模块220在执行根据末端执行器的当前位姿以及目标点，确定停止路径步骤之前或同时：控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动。

在一些实施例中，执行模块220控制机械臂使末端执行器沿末端执行器的轴线，向末端执行器与机械臂的连接端方向移动，移动距离大于或等于1cm。

在一些实施例中，执行模块220还可以：响应于外部停止信号，停止机械臂。

关于上述实施例中的手术机器人的机械臂控制装置200，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

基于同一发明构思，如图12所示，本公开还提供了一种电子设备300，其中，电子设备包括：存储器301、处理器302、输入/输出(Input/Output，I/O)接口303及存储在存储器上并可在处理器上运行的数据处理程序，数据处理程序被处理器执行时实现如前述任一实施例所述的手术机器人的机械臂控制方法100。其中，存储器301，用于存储指令。处理器302，用于调用存储器301存储的指令执行本公开实施例的手术机器人的机械臂控制方法。其中，处理器302分别与存储器301、I/O接口303连接，例如可通过总线系统和/或其他形式的连接机构(未示出)进行连接。存储器301可用于存储程序和数据，包括本公开实施例中涉及的手术机器人的机械臂控制方法的程序，处理器302通过运行存储在存储器301的程序从而执行电子设备300的各种功能应用以及数据处理。

本公开实施例中的存储器301可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(Random Access Memory，RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(Read-OnlyMemory，ROM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，HDD)或固态硬盘(Solid-State Drive，SSD)等。

本公开实施例中处理器302可以采用数字信号处理器(Digital SignalProcessing，DSP)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)、可编程逻辑阵列(Programmable Logic Array，PLA)中的至少一种硬件形式来实现，所述处理器302可以是中央处理单元(Central Processing Unit，CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元中的一种或几种的组合。

本公开实施例中，I/O接口303可用于接收输入的指令(例如数字或字符信息，以及产生与电子设备300的用户设置以及功能控制有关的键信号输入等)，也可向外部输出各种信息(例如，图像或声音等)。本公开实施例中I/O接口303可包括物理键盘、功能按键(比如音量控制按键、开关按键等)、鼠标、操作杆、轨迹球、麦克风、扬声器、和触控面板等中的一个或多个。

可以理解的是，本公开实施例中尽管在附图中以特定的顺序描述操作，但是不应将其理解为要求按照所示的特定顺序或是串行顺序来执行这些操作，或是要求执行全部所示的操作以得到期望的结果。在特定环境中，多任务和并行处理可能是有利的。

基于同一发明构思，根据另一些示例性实施例，本公开还提供了一种计算机可读取介质，该计算机可读取介质存储有如下程序，程序用于执行如前任一实施例所述的手术机器人的机械臂控制方法100。

本公开实施例涉及的方法和装置能够利用标准编程技术来完成，利用基于规则的逻辑或者其他逻辑来实现各种方法步骤。还应当注意的是，此处以及权利要求书中使用的词语“装置”和“模块”意在包括使用一行或者多行软件代码的实现和/或硬件实现和/或用于接收输入的设备。

此处描述的任何步骤、操作或程序可以使用单独的或与其他设备组合的一个或多个硬件或软件模块来执行或实现。在一个实施方式中，软件模块使用包括包含计算机程序代码的计算机可读介质的计算机程序产品实现，其能够由计算机处理器执行用于执行任何或全部的所描述的步骤、操作或程序。

出于示例和描述的目的，已经给出了本公开实施的前述说明。前述说明并非是穷举性的也并非要将本公开限制到所公开的确切形式，根据上述教导还可能存在各种变形和修改，或者是可能从本公开的实践中得到各种变形和修改。选择和描述这些实施例是为了说明本公开的原理及其实际应用，以使得本领域的技术人员能够以适合于构思的特定用途来以各种实施方式和各种修改而利用本公开。

本申请使用了特定词语来描述本申请的实施例。如“一个实施例”、“一实施例”、和/或“一些实施例”意指与本申请至少一个实施例相关的某一特征、结构或特点。因此，应强调并注意的是，本说明书中在不同位置两次或多次提及的“一实施例”或“一个实施例”或“一替代性实施例”并不一定是指同一实施例。此外，本申请的一个或多个实施例中的某些特征、结构或特点可以进行适当的组合。

本申请的上下文中，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。

同理，应当注意的是，为了简化本申请披露的表述，从而帮助对一个或多个申请实施例的理解，前文对本申请实施例的描述中，有时会将多种特征归并至一个实施例、附图或对其的描述中。但是，这种披露方法并不意味着本申请对象所需要的特征比权利要求中提及的特征多。实际上，实施例的特征要少于上述披露的单个实施例的全部特征。

上文已对基本概念做了描述，显然，对于本领域技术人员来说，上述披露仅仅作为示例，而并不构成对本申请的限定。虽然此处并没有明确说明，本领域技术人员可能会对本申请进行各种修改、改进和修正。该类修改、改进和修正在本申请中被建议，所以该类修改、改进、修正仍属于本申请实施例的精神和范围。

Claims (12)

1.一种手术机器人的机械臂控制方法，其中，所述机械臂控制方法包括：

基于所述机械臂末端执行器的位姿信息与目标点，确定偏差，其中，所述目标点为根据手术切口确定的所述末端执行器的远端运动中心；

若所述偏差大于或等于第一阈值，则直接停止所述机械臂；

若所述偏差大于或等于第二阈值，且小于所述第一阈值，则将所述末端执行器停至所述目标点，其中，所述第二阈值小于所述第一阈值。

2.根据权利要求1所述的手术机器人的机械臂控制方法，其中，所述基于所述机械臂末端执行器的位姿信息与目标点，确定偏差，包括：

获取所述末端执行器的当前位姿，及所述当前位姿之前的一个或多个时刻的历史位姿；

分别确定所述当前位姿及所述一个或多个时刻的历史位姿与所述目标点之间的偏移量，其中，所述偏移量为所述目标点到任一时刻的所述末端执行器轴线的距离；

基于全部所述偏移量，确定所述偏差。

3.根据权利要求2所述的手术机器人的机械臂控制方法，其中，所述获取所述末端执行器的当前位姿，及所述当前位姿之前的一个或多个时刻的历史位姿，包括：

获取所述末端执行器当前时刻的位姿信息，作为所述当前位姿；

将所述当前时刻之前n个时刻的位姿信息作为所述历史位姿，其中，n为整数且10≤n≤30。

4.根据权利要求2所述的手术机器人的机械臂控制方法，其中，所述基于全部所述偏移量，确定所述偏差，包括：

基于全部所述偏移量计算加权平均值，确定所述偏差，其中，所述当前位姿对应的偏移量的权重，大于与所述当前位姿时间间隔最远的历史位姿对应的偏移量的权重。

5.根据权利要求1所述的手术机器人的机械臂控制方法，其中，所述第二阈值大于或等于3mm，且小于或等于7mm；所述第一阈值大于或等于8mm，且小于或等于12mm。

6.根据权利要求1所述的手术机器人的机械臂控制方法，其中，所述将所述末端执行器停至所述目标点，包括：

根据所述末端执行器的当前位姿以及所述目标点，确定停止路径；

基于所述停止路径控制所述机械臂，使所述末端执行器停至所述目标点。

7.根据权利要求6所述的手术机器人的机械臂控制方法，其中，在执行所述根据所述末端执行器的当前位姿以及所述目标点，确定所述停止路径步骤之前或同时，所述将所述末端执行器停至所述目标点，还包括：

控制所述机械臂使所述末端执行器沿所述末端执行器的轴线，向所述末端执行器与所述机械臂的连接端方向移动。

8.根据权利要求7所述的手术机器人的机械臂控制方法，其中，所述控制所述机械臂使所述末端执行器沿所述末端执行器的轴线，向所述末端执行器与所述机械臂的连接端方向移动，移动距离大于或等于1cm。

9.根据权利要求1所述的手术机器人的机械臂控制方法，其中，所述手术机器人的机械臂控制方法还包括：

响应于外部停止信号，停止所述机械臂。

10.一种手术机器人的机械臂控制装置，其中，所述机械臂控制装置包括：

偏差确定模块，用于基于所述机械臂末端执行器的位姿信息与目标点，确定偏差，其中，所述目标点为根据手术切口确定的所述末端执行器的远端运动中心；

执行模块，在所述偏差大于或等于第一阈值情况下，用于直接停止所述机械臂；在所述偏差大于或等于第二阈值，且小于第一阈值情况下，用于将所述末端执行器停至所述目标点，其中，所述第二阈值小于所述第一阈值。

11.一种电子设备，其中，所述电子设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的数据处理程序，所述数据处理程序被所述处理器执行时实现如权利要求1-9任一项所述的手术机器人的机械臂控制方法。

12.一种计算机可读取介质，该计算机可读取介质存储有如下程序，所述程序用于执行如权利要求1-9任一项所述的手术机器人的机械臂控制方法。