CN112587243A - 手术机器人及其控制方法、控制装置 - Google Patents

Abstract

本发明涉及一种手术机器人及其控制方法、控制装置。该控制方法包括：获取末端器械的当前位置，并初始化目标位置和指令位置为当前位置；获取末端器械的增量位置；根据增量位置更新目标位置；在目标位置没有达到末端器械的位置限位时，更新指令位置为目标位置，并计算相邻时刻指令位置之间的位置偏差；在位置偏差达到位置偏差阈值时，修正指令位置获得使位置偏差减小的修正指令位置；控制第一部分运动以使末端器械运动至修正指令位置。该控制方法能够减小甚至避免末端器械在受控运动过程中存在的位置跳变的问题，进而可确保手术被可靠实施。

Description

手术机器人及其控制方法、控制装置

技术领域

本发明涉及医疗器械领域，特别是涉及一种手术机器人及其控制方法、控制装置。

背景技术

微创手术是指利用腹腔镜、胸腔镜等现代医疗器械及相关设备在人体腔体内部施行手术的一种手术方式。相比传统手术方式微创手术具有创伤小、疼痛轻、恢复快等优势。

随着科技的进步，微创手术机器人技术逐渐成熟，并被广泛应用。手术机器人包括主操作台及从操作设备，从操作设备包括多个操作臂，这些操作臂包括具有图像末端器械的相机臂及具有操作末端器械的手术臂。主操作台包括显示器及手柄。医生在显示器显示的由相机臂提供的视野下操作手柄控制相机臂或手术臂运动。

在控制末端器械运动的过程中，由于操作臂中末端器械当前执行的指令位置可能与期望执行的目标位置存在较大的位置偏差，因而末端器械在受控过程中容易出现位置跳变的问题，这样不被期望的问题的发生容易对手术造成不利影响。

发明内容

基于此，有必要提供一种能够减小或避免末端器械在受控运动过程中存在的位置跳变的问题的手术机器人及其控制方法、控制装置。

一方面，本发明提供了一种的控制方法，所述手术机器人包括驱动臂以及操纵所述驱动臂中第一部分远端位姿变化的运动输入设备，所述第一部分远端具有末端器械，所述控制方法包括如下步骤：获取所述末端器械的当前位置，并初始化目标位置和指令位置为所述当前位置，所述目标位置为期望所述末端器械达到的位置，所述指令位置为所述末端器械实际执行的位置；获取所述末端器械的增量位置；根据所述增量位置更新所述目标位置；在更新后的所述目标位置没有达到所述末端器械的位置限位时，更新所述指令位置为更新后的所述目标位置，并计算相邻时刻所述指令位置之间的位置偏差；在所述位置偏差达到位置偏差阈值时，修正更新后的所述指令位置获得使所述位置偏差减小的修正指令位置；控制所述第一部分运动以使所述末端器械运动至所述修正指令位置。

其中，所述控制方法还包括：在更新后的所述目标位置达到所述末端器械的位置限位时，停止更新所述指令位置。

其中，所述控制方法还包括：在所述位置偏差低于所述位置偏差阈值时，控制所述第一部分运动以使所述末端器械运动至更新后的所述指令位置。

其中，修正更新后的所述指令位置获得使所述位置偏差减小的修正指令位置的步骤具体为：根据所述位置偏差的一定比例值对更新前的所述指令位置及/或更新后的所述目标位置进行修正从而获得所述修正指令位置。

其中，修正更新后的所述指令位置获得使所述位置偏差减小的修正指令位置的步骤包括：获取所述末端器械在更新前的所述指令位置的第一运动状态及在更新后的所述目标位置的第二运动状态；根据所述第一运动状态及所述第二运动状态规划所述末端器械的第一运动轨迹；对所述第一运动轨迹进行时间分割插补获得所述修正指令位置。

其中，所述第一运动状态包括所述末端器械在更新前的所述指令位置的位置、速度及加速度，所述第二运动状态包括所述末端器械在更新后的所述目标位置的位置、速度及加速度。

其中，所述第一运动轨迹是S曲线、三次曲线或五次曲线。

其中，获取所述末端器械在更新前的所述指令位置的第一运动状态及在更新后的所述目标位置的第二运动状态具体为：获取所述末端器械在更新前的所述指令位置的各位置自由度的第一运动状态及在更新后的所述目标位置的各位置自由度的第二运动状态；根据所述第一运动状态及所述第二运动状态规划所述末端器械的第一运动轨迹具体为：根据各所述位置自由度的所述第一运动状态及所述第二运动状态规划所述末端器械在相应所述位置自由度的第一运动轨迹；对所述第一运动轨迹进行时间分割插补获得所述修正指令位置具体为：对各所述位置自由度的所述第一运动轨迹进行时间分割插补获得相应所述位置自由度的所述修正指令位置。

其中，获取所述末端器械的当前姿态，并初始化目标姿态和指令姿态为所述当前姿态，所述目标姿态为期望所述末端器械达到的姿态，所述指令姿态为所述末端器械实际执行的姿态；获取所述末端器械更新后的所述目标姿态；更新所述指令姿态为更新后的所述目标姿态；控制所述第一部分运动以使所述末端器械运动至更新后的所述指令姿态。

其中，获取所述末端器械的当前姿态，并初始化目标姿态和指令姿态为所述当前姿态，所述目标姿态为期望所述末端器械达到的姿态，所述指令姿态为所述末端器械实际执行的姿态；获取所述末端器械更新后的所述目标姿态；在更新后的所述目标位置没有达到所述位置限位时，更新所述指令姿态为更新后的所述目标姿态，并计算相邻时刻所述指令姿态之间的姿态偏差；在所述姿态偏差达到姿态偏差阈值时，修正更新后的所述指令姿态获得使所述姿态偏差减小的修正指令姿态；控制所述第一部分运动以使所述末端器械运动至所述修正指令姿态。

其中，在更新后的所述目标位置达到所述末端器械的位置限位时，停止更新所述指令姿态。

其中，所述控制方法还包括如下步骤：获取所述末端器械的当前姿态，并初始化目标姿态和指令姿态为所述当前姿态，所述目标姿态为期望所述末端器械达到的姿态，所述指令姿态为所述末端器械实际执行的姿态；获取所述末端器械更新后的所述目标姿态；在更新后的所述目标姿态没有达到所述末端器械的姿态限位时，更新所述指令姿态为更新后的所述目标姿态，并计算相邻时刻所述指令姿态之间的姿态偏差；在所述姿态偏差达到姿态偏差阈值时，修正更新后的所述指令姿态获得使所述姿态偏差减小的修正指令姿态；控制所述第一部分运动以使所述末端器械运动至所述修正指令姿态。

其中，所述控制方法还包括：在更新后的所述目标姿态达到所述末端器械的姿态限位时，停止更新所述指令姿态。

其中，所述控制方法还包括：在所述姿态偏差没有达到姿态偏差阈值时，控制所述第一部分运动以使所述末端器械运动至更新后的所述指令姿态。

其中，修正更新后的所述指令姿态获得使所述姿态偏差减小的修正指令姿态的步骤具体为：根据所述姿态偏差的一定比例值对更新前的所述指令姿态及/或更新后的所述目标姿态进行补偿从而获得所述修正指令姿态。

其中，修正更新后的所述指令姿态获得使所述姿态偏差减小的修正指令姿态的步骤包括：获取所述末端器械在更新前的所述指令姿态的第三运动状态及在更新后的所述目标姿态的第四运动状态；根据所述第三运动状态及所述第四运动状态规划所述末端器械的第二运动轨迹；对所述第二运动轨迹进行时间分割插补获得所述修正指令姿态。

其中，所述第三运动状态包括所述末端器械在更新前的所述指令姿态的姿态、速度及加速度，所述第四运动状态包括所述末端器械在更新后的所述目标姿态的姿态、速度及加速度。

其中，所述第二运动轨迹是S曲线、三次曲线或五次曲线。

其中，获取所述末端器械在更新前的所述指令姿态的第三运动状态及在更新后的所述目标姿态的第四运动状态具体为：获取所述末端器械在更新前的所述指令姿态的各姿态自由度的第三运动状态及在更新后的所述目标姿态的各姿态自由度的第四运动状态；根据所述第三运动状态及所述第四运动状态规划所述末端器械的第二运动轨迹具体为：根据各所述姿态自由度的所述第三运动状态及所述第四运动状态规划所述末端器械在相应所述姿态自由度的第二运动轨迹；对所述第二运动轨迹进行时间分割插补获得所述修正指令姿态具体为：对各所述姿态自由度的所述第二运动轨迹进行时间分割插补获得相应所述姿态自由度的所述修正指令姿态。其中，所述姿态限位是任务空间的姿态限位。

其中，所述姿态限位是所述末端器械基于所述第一部分中各关节组件联动所能获得的关联于姿态的最大运动范围。

其中，所述姿态限位是基于所述最大运动范围所确定的包含于所述最大运动范围以内的运动范围。

其中，所述末端器械的位姿变化由所述第一部分中各关节组件的运动而确定，所述姿态限位是关节空间的所述第一部分中各关节组件的关节限位，所述控制方法包括：实时获取所述第一部分中各关节组件的关节变量；通过判断各所述关节组件对应的所述关节变量是否达到所述关节限位以确定更新后的所述目标姿态是否达到所述姿态限位。

其中，所述姿态限位是所述第一部分中影响所述末端器械的姿态变化的各关节组件的姿态限位。

其中，所述位置限位是任务空间的位置限位。

其中，所述位置限位是所述末端器械基于所述第一部分中各关节组件联动所能获得的关联于位置的最大运动范围。

其中，所述位置限位是基于所述最大运动范围所确定的包含于所述最大运动范围以内的运动范围。

其中，所述末端器械的位姿变化由所述第一部分中各关节组件的运动而确定，所述位置限位是关节空间的所述第一部分中各关节组件的关节限位，所述控制方法包括：实时获取所述第一部分中各关节组件的关节变量；通过判断各所述关节组件对应的所述关节变量是否达到所述关节限位以确定更新后的所述目标位置是否达到所述位置限位。

其中，所述末端器械包括图像末端器械和操作末端器械，受控制的末端器械是所述操作末端器械，对于所述操作末端器械而言，所述位置限位可以基于所述图像末端器械在参考坐标系的可视区域而确定，其中，所述可视区域基于所述图像末端器械的相机参数而确定，所述相机参数包括视场角和景深。

其中，所述位置限位是所述图像末端器械在参考坐标系的可视区域；或者，所述位置限位是包含于所述可视区域以内的区域。

其中，所述控制方法包括：获取所述驱动臂的构型的描述信息；根据所述描述信息生成含有关联于所述驱动臂各部分结构的控件的配置界面以供配置所述第一部分。

其中，所述配置界面含有根据所述描述信息生成的关联于所述驱动臂的模型图像，所述模型图像中含有对应于所述驱动臂的各部分的所述控件或对应于所述驱动臂的各部分中的各关节组件的所述控件。

另一方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被配置为由处理器加载并执行实现如上述任一项实施例所述的控制方法的步骤。

另一方面，本发明提供了一种手术机器人的控制装置，包括：存储器，用于存储计算机程序；及处理器，用于加载并执行所述计算机程序；其中，所述计算机程序被配置为由所述处理器加载并执行实现如上述任一项实施例所述的控制方法的步骤。

另一方面，本发明提供了一种手术机器人，包括：操作臂，所述操作臂包括相机臂及手术臂；以及控制器，所述控制器与所述操作臂耦接，并被配置成执行如上述任一项实施例所述的控制方法的步骤。

其中，所述驱动臂包括机械臂及操作臂，所述操作臂近端装设于所述机械臂远端，所述末端器械装设于所述操作臂远端，所述第一部分是所述操作臂，或者，所述第一部分是所述机械臂和所述操作臂。

其中，所述驱动臂包括机械臂、调整臂、操纵器及操作臂，所述调整臂近端装设于所述机械臂远端，所述操纵器近端装设于所述调整臂远端，所述操作臂近端装设于所述操纵器远端，所述末端器械装设于所述操作臂远端，所述第一部分是所述操作臂，或者，所述第一部分是所述操纵器和所述操作臂，或者，所述第一部分是所述机械臂、所述调整臂、所述操纵器和所述操作臂。

其中，所述运动输入设备是非机动式的运动输入设备。

其中，所述运动输入设备是磁导航式的运动输入设备。

其中，所述运动输入设备是机动式的运动输入设备。

本发明的手术机器人及其控制方法、控制装置，具有如下有益效果：

通过将末端器械实际执行到达的指令位置与相邻时刻期望末端器械到达的目标位置之间的位置偏差与位置偏差阈值进行比较，并在该位置偏差达到位置偏差阈值时，连续地对指令位置进行修正从而获得使位置偏差减小修正指令位置，进而控制驱动臂中第一部分运动以使末端器械运动至修正指令位置，能够减小、甚至避免末端器械运动过程中出现的位置跳变的问题，进而可确保手术被可靠实施。

附图说明

图1为本发明手术机器人一实施例的结构示意图；

图2为图1所示手术机器人一实施例的局部示意图；

图3为手术机器人中操作臂与动力部的结构示意图；

图4为本发明手术机器人另一实施例的结构示意图；

图5为图1所示手术机器人的配置第一部分的一实施例的配置界面；

图6为图1所示手术机器人的配置第一部分的另一实施例的配置界面；

图7为末端器械由达到位置限位到解除位置限位时刻的部分轨迹图；

图8～图11分别为手术机器人的控制方法一实施例的流程图；

图12为手术机器人的控制方法中对指令位姿进行修正的原理示意图；

图13～图16分别为手术机器人的控制方法一实施例的流程图；

图17为末端器械在不同区域内运动的示意图；

图18为手术机器人的控制方法一实施例的流程图；

图19为本发明一实施例的手术机器人的控制装置的结构示意图。

具体实施方式

为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的较佳实施方式。但是，本发明可以以许多不同的形式来实现，并不限于本发明所描述的实施方式。相反地，提供这些实施方式的目的是使对本发明的公开内容理解的更加透彻全面。

需要说明的是，当元件被称为“设置于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。当一个元件被认为是“耦接”另一个元件，它可以是直接耦接到另一个元件或者可能同时存在居中元件。本发明所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的，并不表示是唯一的实施方式。本发明所使用的术语“远端”、“近端”作为方位词，该方位词为介入医疗器械领域惯用术语，其中“远端”表示手术过程中远离操作者的一端，“近端”表示手术过程中靠近操作者的一端。本发明所使用的术语“第一/第二”等表示一个部件以及一类具有共同特性的两个以上的部件。

除非另有定义，本发明所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本发明中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本发明。本发明所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。本发明中所使用的术语“各”包括一个或两个以上。

如图1至图2所示，其分别为本发明手术机器人一实施例的结构示意图，及其局部示意图。

手术机器人包括主操作台2及由主操作台2控制的从操作设备3。主操作台2具有运动输入设备21及显示器22，医生通过操作运动输入设备21向从操作设备3发送控制命令，以令从操作设备3根据医生操作运动输入设备21的控制命令执行相应操作，并通过显示器22观察手术区域。其中，从操作设备3具有驱动臂，驱动臂具有机械臂30及可拆卸地装设于机械臂30远端的一个以上的操作臂31。机械臂30包括依次连接的基座及连接组件，连接组件具有多个关节组件。操作臂31包括依次连接的连杆32、连接组件33及末端器械34，其中，连接组件33具有多个关节组件，通过调节操作臂31的关节组件调节末端器械34的姿态；末端器械34具有图像末端器械34A及操作末端器械34B。图像末端器械34A用于采集视野内的图像，显示器22用于显示该图像。操作末端器械34B用于执行手术操作如剪切、缝合。本文令具有图像末端器械34A的操作臂为相机臂31A，并令具有操作末端器械34B的操作臂为手术臂31B。

图1展示的手术机器人为单孔手术机器人，各操作臂31通过装设于机械臂30远端的同一个穿刺器4插入至患者体内。在单孔手术机器人中，医生一般仅对操作臂31进行控制以完成基本手术操作。此时，单孔手术机器人的操作臂31应当同时具有位置自由度(即定位自由度)和姿态自由度(即定向自由度)，以实现在一定范围内位姿的变化，例如操作臂31具有水平移动自由度x、竖直移动自由度y，自转自由度α、俯仰自由度β及偏航自由度γ，操作臂31还可以在机械臂30远端关节组件即动力机构301的驱动下实现前后移动自由度z(即进给自由度)，此外，一些实施例中，还可以为操作臂31设置冗余自由度以实现更多功能的可能性，例如，在上述可实现6自由度的前提下，额外再设置一个、两个甚至更多个自由度。例如，动力机构301具有导轨和滑动设置于导轨上的动力部，操作臂31可拆卸的装设于动力部上，一方面，动力部在导轨上的滑动提供操作臂31前后移动自由度z，另一方面，动力部为操作臂31的关节组件提供动力实现其余5个自由度(即[x,y,α,β,γ])。

手术机器人还包括控制器。控制器可以集成于主操作台2，也可以集成于从操作设备3。当然，控制器也可以独立于主操作台2和从操作设备3，其例如可部署在本地，又例如控制器可以部署在云端。其中，控制器可以由一个以上的处理器构成。

手术机器人还包括输入部。输入部可以集成于主操作台2。输入部也可以集成于从操作设备3。当然，输入部也可以独立于主操作台2和从操作设备3。该输入部例如可以是鼠标、键盘、语音输入装置、触摸屏。一实施例中，采用触摸屏作为输入部，触摸屏例如可以设置于主操作台2的扶手上。

操作臂31还包括感应关节组件的关节变量的传感器。这些传感器包括感应关节组件转动运动的角度传感器及感应关节组件线性运动的位移传感器，具体可根据关节组件的类型来配置适应的传感器。

控制器与这些传感器耦接，并与输入部及显示器22耦接。

示例性的，如图3所示，操作臂31的驱动盒310抵接于动力机构301的动力部302的抵接面装设有存储单元311，相应在动力部302抵接于驱动盒310的抵接面装设有与存储单元311配套的读取单元303，该读取单元303与控制器耦接，操作臂31装设于动力部302时，读取单元303与存储单元311通讯，读取单元303从存储单元311中读取相关信息。该存储单元311例如是存储器、电子标签。存储单元例如存储有操作臂的类型、操作臂可被配置成目标部位的部位、操作臂的运动学模型等。例如，相机臂31A的存储单元311中还额外的存储有相机参数。

如图4所示，其为本发明手术机器人一实施例的结构示意图，更具体地，图4所展示的是多孔手术机器人一实施例的结构示意图。图4所示的多孔手术机器人与图1所示的单孔手术机器人之间的区别主要存在于两者的从操作设备之间的区别。图4所示的多孔手术机器人中从操作设备的驱动臂具有依次连接的机械臂110、调整臂120、操纵器130及操作臂150。调整臂120、操纵器130及操作臂150数量相同且均为两个以上，例如四个，机械臂110远端具有定向平台，调整臂120近端均连接于定向平台，操纵器130近端连接于调整臂120远端。操纵器130用于可拆卸地连接操作臂150，操纵器130具有多个关节组件。在多孔手术机器人中，不同操作臂150通过不同的穿刺器插入患者体内，多孔手术机器人的操作臂150相较于单孔手术机器人的操作臂31而言，一般具有较少的自由度，通常，操作臂150仅具有姿态自由度(即定向自由度)，当然其姿态的变化一般也对位置产生影响，但因为影响较小某些场景下可以被忽略。操作臂150的位置的变化通常可以由操纵器130辅助实现，由于操纵器130与操作臂150联动实现位姿变化，可以将这两者认为是操纵器组件，与单孔手术机器人中操作臂31相当。

根据配置，运动输入设备21可以输入包括位置指令及姿态指令的位姿指令以控制驱动臂中第一部分远端位姿的变化。该第一部分远端通常指末端器械，此外，该第一部分远端还可以指与末端器械项连接的一个关节组件，末端器械位姿的变化通常与该关节组件位姿的变化是一致的。

在图1所示的手术机器人中，驱动臂包括机械臂及操作臂，操作臂近端装设于机械臂远端，末端器械装设于操作臂远端。根据配置，第一部分可被配置成是操作臂；或者，第一部分可被配置成是机械臂和操作臂的整体。

而相应在图4所示的手术机器人中，驱动臂包括机械臂、调整臂、操纵器及操作臂，调整臂近端装设于机械臂远端，操纵器近端装设于调整臂远端，操作臂近端装设于操纵器远端，末端器械装设于操作臂远端。根据配置，第一部分可被配置成是操作臂；或者，第一部分可被配置成是操纵器和操作臂的整体；或者，第一部分可被配置成是机械臂、调整臂、操纵器和操作臂的整体。

可理解的，无论是图1所示的单孔手术机器人还是图4所示的多孔手术机器人，机械臂通常用于大范围调节末端器械的位姿，操作臂用于精细调节末端器械的位姿，例如，手术前通过机械臂等摆位，手术中主要通过控制操作臂实施手术。当然，一些实施例中，也可以结合机械臂及操作臂等相应臂体结构一起协同运动实现特定功能。

一实施例中，可以在手术机器人的系统文件中定义出驱动臂中期望作为第一部分所关联的结构，在手术机器人的系统初始化时从系统文件中读取出该第一部分所关联的结构并应用于后述实施例即可。

一实施例中，也可以根据驱动臂的构型的描述信息实时生成用于配置该第一部分的配置界面。该描述信息包括驱动臂中各部分中全部关节组件的连杆参数等。例如，该配置界面含有关联于该驱动臂的各部分结构的、可供选择的控件以供医生配置。该控件例如可以是文本控件、选项控件如下拉列表控件、按钮控件等多种形式。

较佳的，为了更加便于医生更加直观的通过配置界面配置该第一部分，可以根据驱动臂的构型的描述信息生成相关联的、且含有可供选择的控件的模型图像。该模型图像可以是简洁示意的投影模型图像或复杂结构示意的计算机模型。该模型图像可跟随驱动臂状态的变化而变化。当然，该模型图像也可不跟随驱动臂状态的变化而只反映例如初始状态(如关节变量为零位时)等某一时刻下驱动臂的构型。模型图像上的控件例如是图标控件，更具体地可以是光点、光圈等。

示例性的，对于图1所示的手术机器人，驱动臂中的机械臂及操作臂可以分别对应一个控件以供选择它们的整体作为第一部分；对于图4所示的手术机器人，驱动臂中的机械臂、调整臂、操纵器及操作臂可以分别对应一个控件以供选择它们的整体作为第一部分。

示例性的，对于图1及图4所示的手术机器人，驱动臂中各关节组件均可以分别对应一个控件以供选择它们的局部或整体作为第一部分，其中，未被选择的整体或局部被系统视为非铰接结构从而禁止它们运动即可。可以获取医生通过输入部绘制的至少覆盖模型图像中部分控件的封闭的图形，进而将该图形内含有的(即围合的)所有部分全部作为第一部分。这样的设计能够提高第一部分的配置效率。

如图5和图6所示，图5和图6分别示意了图1所示手术机器人的配置第一部分的一实施例的配置界面。在图5和图6中，例如可用图标控件“○”代表可被配置成第一部分中至少部分的部位，并可用图标控件“●”代表被配置成第一部分中至少部分的部位。如图5所示，模型图像基本示意了图1所示的单孔手术机器人的基本构成，其中，机械臂和操作臂Arm1～Arm3分别对应含有一个可供选择的控件，是否选择该控件对应确定了是否将其对应的臂体部分作为第一部分，例如在图5中，仅选择将操作臂Arm1整体配置成第一部分，同时操作臂Arm1远端的末端器械被配置成受控末端器械。如图6所示，模型图像中的机械臂和操作臂Arm1～Arm3分别对应含有多个可供选择的控件，机械臂及操作臂Arm1～Arm3中控件的数量基本与其各自所具有的关节组件的数量相同，每个控件例如可以代表其中一个相应的关节，例如在图5中，由于选中了操作臂Arm3所有控件，进而相当于将操作臂Arm3整体配置成第一部分。

在应用于后续实施例时，预先获取医生根据该配置界面配置的第一部分进而利用该第一部分实现本发明的目的即可。这样的设计可更加易于医生灵活的配置出期望的第一部分以适用不同应用场景。

根据配置，末端器械中的一个以上可以被配置成受控末端器械以接受运动输入设备的控制。

一实施例中，提供一种手术机器人的控制方法，该控制方法可以由控制器执行。结合图7和图8进行参阅，该控制方法包括如下步骤：

步骤S101，获取末端器械的当前位置，并初始化目标位置和指令位置为当前位置。

该目标位置为期望末端器械达到的位置，在本发明中目标位置是实时更新或刷新的，更具体地，目标位置通常由运动输入设备映射给末端器械，更具体地为映射给控制末端器械的控制器，运动输入设备输入的目标位置与末端器械获得的目标位置通常具有一定的比例关系，该比例关系可被配置。一些实施例中，运动输入设备的运动自由度与末端器械的运动自由度为自然映射关系，例如，运动输入设备在X轴方向、Y轴方向、Z轴方向的自由度一一对应于末端器械在X轴方向、Y轴方向、Z轴方向的自由度。一些实施例中，运动输入设备的运动自由度与末端器械的运动自由度为非自然映射关系，即不满足自然映射关系的其它映射关系。

指令位置为下发给末端器械来实际执行的位置，在本发明中指令位置来源于目标位置中的部分或全部，具体可以根据约束条件而确定。

当前位置存在多种来源途径，例如，当前位置可以来源于系统实时记录的末端器械的实际位置。又例如，当前位置可以根据实时计算的结果获取，比如根据驱动臂中第一部分的运动学模型及实时检测获得的第一部分中各关节组件的关节变量进行正运动学求解得出，其中，这些关节变量通常指关节组件中转动关节组件的关节转动量及/或移动关节组件的关节偏移量。

其中，初始化目标位置和指令位置为当前位置可理解为，对目标位置和指令位置进行初始化，具体的，用当前位置初始化目标位置及指令位置。其中，至少该目标位置会随着医生的后续操作而被更新。

步骤S102，获取末端器械的增量位置。

在主从式机器人系统中，末端器械通常基于运动输入设备的控制进行增量式运动。例如，该增量位置可以通过计算相邻时刻的目标位置之间的差值而求解获得。

步骤S103，根据增量位置更新目标位置。

具体而言，根据目标位置及增量位置更新该目标位置。可见，目标位置总是处于更新状态。

步骤S104，判断目标位置是否达到末端器械的位置限位。

该位置限位通常是一个或多个数值范围(也可理解为数值集合)，“达到”或“到达”或“超过”，通常可以表示等于及/或大于的含义。其中，被用于判断的目标位置通常指得是更新后的目标位置，以供在后续决策是否需要更新指令位置为该更新后的目标位置。

在该步骤S104中，如果目标位置达到末端器械的位置限位，则进入步骤S105；而如果目标位置没有达到末端器械的位置限位，则进入步骤S106。例如，如图7所示，P1为当前时刻的指令位置，P2为下一时刻或历经多个时刻的目标位置，其中，P1达到位置限位，P1到P2之间的全部时刻所对应的目标位置不会被更新成指令位置；P2未达到(即解除了)位置限位，P2所对应的时刻将被更新成指令位置。

步骤S105，停止更新指令位置。

根据该步骤S105，由于目标位置达到位置限位通常表明不能再对末端器械的位置进行调整，故指令位置不再更新，因而末端器械保持于指令位置处。在该步骤S105之后，重新进入步骤S102。

步骤S106，更新指令位置为目标位置，并计算相邻时刻指令位置之间的位置偏差。

根据该步骤S106，由于目标位置没有达到位置限位通常表明还可以对末端器械的位置进行调整，故可以将指令位置更新为目标位置。此时，计算相邻时刻指令位置之间的位置偏差即可以获得由于位置限位的影响而造成的末端器械不能跟随运动输入设备运动的偏差距离。

步骤S107，判断位置偏差是否达到位置偏差阈值。

在该步骤S107中，如果位置偏差达到位置偏差阈值，则进入步骤S108；而如果位置偏差低于位置偏差阈值，则进入步骤S110。如图7所示P2与P1之间的位置偏差未达到位置偏差阈值，则直接控制第一部分运动到P2；而如果P2与P1之间的位置偏差达到位置偏差阈值，则需要对P2进行修正以获得P3，其中，P3为修正指令位置，进而将控制第一部分运动不达到P2而达到P3。

步骤S108，修正指令位置获得使位置偏差减小的修正指令位置。

这里即对更新后的指令位置进行修改，修改成合适的即能够使位置偏差减小的新的指令位置，简单定义其为修正指令位置。通常，每次执行步骤S108时，对于指令位置的修正通常只进行一次，也即每次只修正一些指令位置。如果修正后的指令位置与相邻时刻的指令位置之间仍然存在超过位置偏差阈值的位置偏差，留待下一次进行修正即可。

对于该位置偏差的修正量，通常要求其不能大于位置偏差阈值，以避免矫枉过正的情况。通过约束，该修正量至多等于位置偏差阈值。较优的，该修正量小于位置偏差阈值，以实现更细微的调整。

步骤S109，控制第一部分运动以使末端器械运动至修正指令位置。

在该步骤S109中，通过结合第一部分的运动学模型及逆运动学可以将修正指令位置解析成第一部分中各关节组件的关节变量进而便于对第一部分的运动的控制。在该步骤S109之后，重新进入步骤S102。

步骤S110，控制第一部分运动以使末端器械运动至指令位置。

在该步骤S110中，通过逆运动学可以将指令位置解析成第一部分中各关节组件的关节变量进而便于对第一部分的运动的控制。在该步骤S110之后，重新进入步骤S102。

末端器械从上一时刻的指令位置移动至修正指令位置，相较于末端器械从上一时刻的指令位置移动至指令位置而言，相邻时刻指令位置之间的位置偏差被缩小了，通过重复上述步骤S102～步骤S109，末端器械最终能够在某个时刻直接跟随不经修正的指令位置运动而基本消除相邻时刻位置跳变的问题。

在其它实施例中，即使目标位置没有达到该位置限位，但相邻时刻指令位置之间的位置偏差仍大于位置偏差阈值，也可以对该指令位置进行修正以获得使位置偏差减小的修正指令位置，进而根据修正指令位置控制第一部分运动以基本消除位置跳变的不利影响。

上述驱动臂中的第一部分可以被自由配置。一实施例中，驱动臂中第一部分对应的运动学模型通常可以由控制器实时构建。例如，获取该第一部分所含有的全部关节组件及其连杆参数，进而基于这些连杆参数构建对应于该第一部分的运动学模型。一实施例中，驱动臂中所有关节组件的用于构建运动学模型的连杆参数等相关信息均可以存储于操作臂的存储单元中。另一实施例中，驱动臂中除操作臂以外的关节组件的用于构建运动学模型的连杆参数等相关信息可以存储于不同于操作臂的存储单元且与控制器耦接的另一存储单元中。该另一存储单元可以集成或者独立于控制器。

在末端器械达到位置限位的条件下，对于末端器械的姿态的控制可以被配置成采用以下两种策略中的任意一种来进行。

<策略一>如图9所示，本发明的控制方法还可以包括以下步骤：

步骤S201，获取末端器械的当前姿态，并初始化目标姿态和指令姿态为当前姿态。

相应于对目标位置及指令位置的定义，该目标姿态为期望末端器械达到的姿态，在本发明中目标姿态是实时更新或刷新的，更具体地，目标姿态通常由运动输入设备映射给末端器械，更具体地为映射给控制末端器械的控制器，运动输入设备输入的目标姿态与末端器械获得的目标姿态通常是完全一致的(即两者取向对齐)，既不需要进行增量运动也不需要进行比例缩放，而且通常它们可以满足自然映射关系。

其中，初始化目标姿态和指令姿态为当前姿态可理解为，对目标姿态和指令姿态进行初始化，具体的，用当前姿态初始化目标姿态及指令姿态。其中，至少该目标姿态会随着医生的后续操作而被更新。

步骤S202，获取末端器械的目标姿态。

步骤S203，判断目标位置是否达到末端器械的位置限位。

在该步骤S203中，如果目标位置达到位置限位，进入步骤S204；而如果目标位置没有达到位置限位，则进入步骤S205。

步骤S204，停止更新指令姿态。

根据该步骤S204，由于目标位置达到位置限位通常表明不能再对末端器械的位置进行调整，故可以一并地不再更新指令姿态，以使末端器械在到达位置限位时保持位姿。在该步骤S204之后，重新进入步骤S202。

步骤S205，更新指令姿态为目标姿态，并计算相邻时刻指令姿态之间的姿态偏差。

根据该步骤S205，由于末端器械在没有达到位置限位时，可以自由挪动，故通常也不需要限制末端器械的姿态，故可以将指令姿态更新为目标姿态。此时，计算相邻时刻指令姿态之间的姿态偏差即可以获得由于姿态限位的影响而造成的末端器械不能跟随运动输入设备运动的偏差角度。

步骤S206，判断姿态偏差是否达到姿态偏差阈值。

在该步骤S206中，如果姿态偏差达到姿态偏差阈值，进入步骤S207；而如果姿态偏差没有达到姿态偏差阈值，进入步骤S209。

步骤S207，修正指令姿态获得使姿态偏差减小的修正指令姿态。

这里即对更新后的指令姿态进行修改，修改成合适的即能够使姿态偏差减小的新的指令姿态，简单定义其为修正指令姿态。通常，每次执行步骤S207时，对于指令姿态的修正通常只进行一次，也即每次只修正一些指令姿态。如果修正后的指令姿态与相邻时刻的指令姿态之间仍然存在超过姿态偏差阈值的姿态偏差，留待下一次进行修正即可。

对于该姿态偏差的修正量，通常要求其不能大于姿态偏差阈值，以避免矫枉过正的情况。通过约束，该修正量至多等于姿态偏差阈值。较优的，该修正量小于姿态偏差阈值，以实现更细微的调整。

步骤S208，控制第一部分运动以使末端器械运动至修正指令姿态。

在该步骤S208中，通过结合第一部分的运动学模型及逆运动学可以将修正指令姿态解析成第一部分中各关节组件的关节变量进而便于对第一部分的运动的控制。在该步骤S208之后，重新进入步骤S202。

步骤S209，控制第一部分运动以使末端器械运动至指令姿态。

在该步骤S209中，通过逆运动学可以将指令姿态解析成第一部分中各关节组件的关节变量进而便于对第一部分的运动的控制。在该步骤S209之后，重新进入步骤S202。

末端器械从上一时刻的指令姿态移动至修正指令姿态，相较于末端器械从上一时刻的指令姿态移动至指令姿态而言，相邻时刻指令姿态之间的姿态偏差被缩小了，通过重复上述步骤S202～步骤S208，末端器械最终能够在某个时刻直接跟随不经修正的指令姿态运动而基本消除相邻时刻姿态跳变的问题，实现优异的跟随效果。

在其它实施例中，即使目标姿态没有达到该姿态限位，但相邻时刻指令姿态之间的姿态偏差仍大于姿态偏差阈值，也可以对该指令姿态进行修正以获得使姿态偏差减小的修正指令姿态，进而根据修正指令姿态控制第一部分运动以基本消除姿态跳变的不利影响。

<策略二>如图10所示，本发明的控制方法还可以包括以下步骤：

步骤S301，获取末端器械的当前姿态，并初始化目标姿态和指令姿态为当前姿态。

对于目标姿态及指令姿态的定义与本文其他实施例中对他们的定义是保持一致的，故此处不再重复描述。

步骤S302，获取末端器械的目标姿态。

步骤S303，更新指令姿态为目标姿态。

在该步骤S303中，无论末端器械是否达到位置限位，均更新指令姿态为目标姿态。也即，即使末端器械的目标位置达到了位置限位，本发明也允许末端器械的姿态跟随运动输入设备输入的姿态运动，进而能够始终保持末端器械与运动输入设备的取向对齐，避免了某些场景例如末端器械达到位置限位时通常要求末端器械位姿均保持的情况下，恢复对末端器械的控制时要对齐末端器械与运动输入设备之间的取向的繁杂步骤，这对于非机动式的运动输入设备而言尤为适用。

步骤S304，控制第一部分运动以使末端器械运动至指令姿态。

本发明还提供另外一种实施例的控制方法，该控制方法可以独立实施，也可以结合前文所描述的实施例一起实施。如图11所示，该控制方法包括如下步骤：

步骤S401，获取末端器械的当前姿态，并初始化目标姿态和指令姿态为当前姿态。

步骤S402，获取末端器械的目标姿态。

步骤S403，判断目标姿态是否达到末端器械的姿态限位。

该姿态限位通常也是一个或多个数值范围(也可理解为数值集合)。其中，被用于判断的目标姿态通常指得是更新后的目标姿态，以供在后续决策是否需要更新指令姿态为该更新后的目标姿态。

在该步骤S107中，如果目标姿态达到末端器械的姿态限位，进入步骤S404；而如果目标姿态没有达到末端器械的姿态限位，进入步骤S405。

步骤S404，停止更新指令姿态。

根据该步骤S404，由于目标姿态达到姿态限位通常表明不能再对末端器械的姿态进行调整，故指令姿态不再更新，因而末端器械保持于指令姿态处。在该步骤S404之后，重新进入步骤S402。

步骤S405，更新指令姿态为目标姿态，并计算相邻时刻指令姿态之间的姿态偏差。

根据该步骤S405，由于目标姿态没有达到姿态限位通常表明还可以对末端器械的姿态进行调整，故可以将指令姿态更新为目标姿态。此时，计算相邻时刻指令姿态之间的姿态偏差即可以获得由于姿态限位的影响而造成的末端器械不能跟随运动输入设备运动的偏差角度。

步骤S406，判断姿态偏差是否达到姿态偏差阈值。

在该步骤S406中，如果姿态偏差达到姿态偏差阈值，则进入步骤S407；而如果姿态偏差低于姿态偏差阈值，则进入步骤S409。

步骤S407，修正指令姿态获得使姿态偏差减小的修正指令姿态。

步骤S408，控制第一部分运动以使末端器械运动至修正指令姿态。

并在该步骤S408之后，重新进入步骤S402。

步骤S409，控制第一部分运动以使末端器械运动至指令姿态。

并在该步骤S409之后，重新进入步骤S402。

一些实施例中，对于上述指令位置及/或指令姿态(即指令位姿)的修正示例性的均可以采用如下两种方式。

<方式一>

可以根据位姿偏差的一定比例值对指令位姿及/或目标位姿进行补偿从而获得修正指令位姿。其中，该位姿偏差包括位置偏差及/或姿态偏差，该指令位姿包括指令位置及/或指令姿态，该目标位姿包括目标位置及/或目标姿态。

示例性的，可以将位姿偏差的一定比例值作为修正量对指令位姿进行修正而获得修正指令位姿。

示例性的，可以将位姿偏差的一定比例值作为修正量对目标位姿进行修正而获得修正指令位姿。

示例性的，可以将位姿偏差的第一比例值作为第一修正量对指令位姿进行修正、同时将位姿偏差的第二比例值作为第二修正量对目标位姿进行修正而获得修正指令位姿。该第一比例值与第二比例值可以相同或不同。

其中，用于对指令位姿及/或目标位姿的进行修正的比例值可以是一个设定的定值，也可以是根据位姿偏差而实时确定的可变值。为可变值时，位姿偏差较大时，比例值也较大，位姿偏差较小时，比例值也较小。

在该方式一中，例如对于如何获得修正指令位置，一方面可以不用考虑各位姿自由度上各自独立的位置偏差，而是作为一个整体修正该整体的位姿偏差即可；另一方面还可以分别考虑各位姿自由度并分别在不同位姿自由度上相应的对其位姿偏差进行修正，而且不同位姿自由度的位姿偏差可以用相同比例值或不同比例值进行修正。该方式一对于如何获得修正指令姿态也可以采用相同或相类似的方法。

在该方式一中，可以基于任务空间如笛卡尔空间进行如上述的比例修正从而实现对指令位姿的修正；此外，也可以基于关节空间(需要获得指令位姿、目标位姿的逆解进而基于各关节的运动状态)进行如上述的比例修正从而实现对指令位姿的修正。

以图12所示为例，按照方式一获得修正指令位姿(可理解成指令位置及/或指令姿态)的简要说明如下：

其中，S0为T0时刻末端器械的当前位姿(即初始化的指令位姿)，M1～M9分别为T1～T9时刻末端器械的目标位姿。S1～S9分别为T1～T9时刻分别所获得的末端器械的修正指令位姿。

可见，随着对指令位姿进行修正的不断进行，指令位姿逐渐逼近甚至重合于目标位姿，进而基本甚至完全避免了指令位姿与目标位姿之间在如启动时的由于位姿偏差导致的位姿跳变。这个过程是连续不间断进行的，无需特意中断操作以先进行位姿对齐，利用运动输入设备正常操纵末端器械即可逐渐实现位姿对齐，可以保证手术实施的连续性。

<方式二>

还可以根据如下方法对指令位姿进行修正而获得该修正指令位姿，具体如图13所示，该方法包括如下步骤：

步骤S501，获取末端器械在指令位姿的运动状态及在目标位姿的运动状态。

其中，运动状态包括需要被考虑的位置、速度及加速度。

步骤S502，根据末端器械在指令位姿的运动状态及末端器械在目标位姿的运动状态规划末端器械的运动轨迹。

可根据需要，例如，可根据在线规划的方式规划该运动轨迹。例如可以将该运动轨迹设计成S型曲线。又例如可以将该运动轨迹设计成多项式曲线如三次曲线或五次曲线。又例如可以将该运动轨迹设计成梯形曲线。例如，可选择具有二阶连续性的五次曲线式的运动轨迹。

步骤S503，对该运动轨迹进行时间分割插补获得修正指令位姿。

根据上述步骤S501～步骤S503，可以使位姿(如位置及/或姿态)的对齐过程更加流畅和平滑，使医生具有更好的操作体验。

在该方式二中，对欲修正的指令位姿(即指令位置及/或指令姿态)通常需要根据其所包含的位姿自由度来单独规划相应位姿自由度上的运动轨迹，从而在相应位姿自由度对应的运动轨迹上进行时间分割插补获得相应修正指令位姿。其中，位姿自由度包括位置自由度及/或姿态自由度。

具体而言，上述步骤S501具体可以为：获取末端器械在指令位姿的各位姿自由度的运动状态及在目标位姿的各位姿自由度的运动状态。上述步骤S502具体可以为：根据各位姿自由度的末端器械在指令位姿的运动状态及末端器械在目标位姿的运动状态规划末端器械在相应位姿自由度的运动轨迹。上述步骤S503具体可以为：对各位姿自由度的运动轨迹进行时间分割插补获得相应位姿自由度的修正指令位姿。

该方式二中，也可以基于任务空间如笛卡尔空间进行在线运动轨迹规划从而实现对指令位姿的修正；当然，也可以基于关节空间(需要获得指令位姿、目标位姿的逆解进而基于各关节的运动状态)进行如上所述的在线运动轨迹规划从而实现对指令位姿的修正。

一些实施例中，位置限位是任务空间的位置限位。示例性的，该任务空间是笛卡尔空间。通常，该位置限位是末端器械基于第一部分中各关节组件联动所能获得的关联于位置的最大运动范围。此外，该位置限位还可以是基于关联于位置的最大运动范围所确定的包含于最大运动范围以内的运动范围。可以理解的，该最大运动范围相当于物理限位，该包含于最大运动范围以内的运动范围相当于软件限位。

一些实施例中，末端器械的位姿变化通常由第一部分中各关节组件的运动而确定，因而该位置限位也可以是关节空间的第一部分中各关节组件的关节限位(即关节最大运动范围)。如图14所示，上述步骤S104，即判断指令位置是否达到末端器械的位置限位的步骤包括：

步骤S1041，实时获取第一部分中各关节组件的关节变量。

这些关节变量可以通过传感器获得，或者也可以根据逆运动学解算获得。

步骤S1042，通过判断各关节组件对应的关节变量是否达到相应关节组件的关节限位以确定指令位置是否达到位置限位。

在该步骤S1042中，通常，在一种策略中，任意一个关节组件的关节变量达到其对应的关节限位时，都可以确定末端器械达到位置限位。在该实施例中的关节限位可以是第一部分中影响受控末端器械的位置变化的各关节组件的关节限位，在上述步骤S1041中实际上仅需要检测这些影响受控末端器械位置变化的关节组件的关节变量，并在上述步骤S1042中将这些关节变量与相应的关节限位进行比较即可。其中，哪些关节组件将会影响受控末端器械的位置具体可根据该第一部分的构型而被准确地确定，因为这通常在设计之初就可以被确定。

简单而言，该位置限位可以是第一部分中影响末端器械的位置变化的各关节组件的位置限位。

一些实施例中，末端器械包括图像末端器械和操作末端器械，根据配置，在受控制的末端器械是操作末端器械时，对于该操作末端器械而言，该操作末端器械的位置限位还可以基于图像末端器械在参考坐标系的可视区域而确定。其中，可视区域基于图像末端器械的相机参数而确定，相机参数包括视场角和景深。视场角关联于焦距，景深关联于光圈。其中，焦距越小，视场角越大，可视距离越近；焦距越大，视场角越小，可视距离越远。例如，利用三角函数公式并结合视场角及景深即可计算出该可视区域。可视区域可以通过实时计算的方式获得，也可以根据视场角及景深直接从预先设定好的数据库如对照表中调取获得。

例如，该位置限位是图像末端器械在参考坐标系的可视区域。又例如，位置限位是包含于可视区域以内的区域，例如，该区域是基于可视区域裁剪得到的适合于主操作台中显示器显示的部分区域。该两种方式的位置限位可以被理解成是软件限位。

事实上，根据获取的可视区域，可以获得一个立体空间，也可以是该立体空间的一个平面区域，例如以f(x,y,z)代表该立体空间的情况下，可以用f(x,y)代表该立体空间中相应景深z所对应的平面区域。其中，通过坐标转换，可以将f(x,y,z)转换为参考坐标系下的f’(x,y,z)，还可以将f(x,y)转换为参考坐标系下的f’(x,y)，进而获得可视区域在参考坐标系的位置范围。

在位置限位是可视区域或与该可视区域相关联的区域的情况下，能够更加直观的、且可靠的帮助医生进行手术操作，比如，可有效防止因为末端器械在可视区域外不期望的运动对患者造成意外伤害。示例性的，末端器械到达可视区域的边界时，至少可限制末端器械受控移出至可视区域以外，而且，即使不限制运动输入设备的运动，在运动输入设备重新回归正常操控末端器械时，末端器械也不会出现位置上的大的跳跃，因而增强了手术实施的安全性及可靠性。

一些实施例中，姿态限位是任务空间的姿态限位。示例性的，该任务空间是笛卡尔空间。通常，该姿态限位是末端器械基于第一部分中各关节组件联动所能获得的关联于姿态的最大运动范围。此外，该姿态限位是基于该关联于姿态的最大运动范围所确定的包含于最大运动范围以内的运动范围。

末端器械的位姿变化由第一部分中各关节组件的运动而确定，因为该姿态限位也可以是关节空间的第一部分中各关节组件的关节限位，其中，该关节空间是由第一部分中各关节组件的所有关节向量组成的空间。如图15所示，上述步骤S203，即判断目标位置是否达到位置限位的步骤包括：

步骤S2031，实时获取第一部分中各关节组件的关节变量。

步骤S2032，通过判断各关节组件对应的关节变量是否达到关节限位以确定指令姿态是否达到姿态限位。

简单而言，该姿态限位可以是第一部分中影响末端器械的姿态变化的各关节组件的姿态限位。

在该步骤S2032中，通常，在一种策略中，任意一个关节组件的关节变量达到其对应的关节限位时，都可以确定末端器械达到位置限位。在该实施例中的关节限位可以是第一部分中影响末端器械的姿态变化的各关节组件的关节限位，在上述步骤S2031中实际上仅需要检测这些影响末端器械位置变化的关节组件的关节变量，并在上述步骤S2032中将这些关节变量与相应的关节限位进行比较即可。

上述实施例的控制方法适用于机动式及非机动式的运动设备，且尤其适用于非机动式的运动设备，该非机动式的运动输入设备包括但不限于磁导航式的运动输入设备。

一实施例中，如图16所示，上述控制第一部分运动的步骤包括：

步骤S601，获取位于位姿限位的安全运动区域，令安全运动区域内的区域为第一区域，令安全运动区域以外、位姿限位以内的区域为第二区域。

该位姿限位是位置限位及/或姿态限位。

步骤S602，根据末端器械在第一区域及第二区域内的位置及运动方向的变化而改变末端器械的运动速度。

其中，末端器械的运动方向通常可以这样进行确定：获取末端器械的当前位置；并获取末端器械在下一时刻的目标位置；进而根据目标位置及当前位置即可确定末端器械的运动方向。

进一步地，末端器械在下一时刻的目标位置例如可以通过如下步骤获取：获取运动输入设备输入的目标位姿信息；根据目标位姿信息计算第一部分中各关节组件的关节变量；获取第一部分的运动学模型；结合运动学模型及各关节变量确定目标部位在下一时刻的目标位置。

在该步骤S602中，即根据末端器械在第一区域及第二区域内位置及运动方向的变化而改变末端器械的运动速度的步骤具体可以这样来实现：

例如，在末端器械从第一区域的边界向第二区域的外边界运动时，减小末端器械在相应方向的运动速度；而在末端器械从第二区域的外边界向第一区域的边界运动时，增大末端器械在相应方向的运动速度。其中，第二区域包括内边界和外边界，第二区域的内边界与第一区域的边界相同，均指安全运动区域的边界，第二区域的外边界指位姿限位的边界。

结合图17参阅，A点位于第一区域内，B点位于第二区域内，C点位于第二区域外，末端器械从A点经过B点向C点运动的整个运动过程中，分为三个阶段，包括从A点到第一区域的边界的第一阶段，从第一区域的边界到第二区域的外边界的第二阶段，及从第二区域的外边界到C点的第三阶段，其中，第一阶段的运动速度为v1，第二阶段的运动速度为v2，第三阶段的运动速度v3，v1>v2>v3，其中，v3＝0，也即实质上整个运动过程仅包括第一阶段和第二阶段两个阶段。继续参阅图17，从C点经过B点向A点运动的整个运动过程实质上也仅包括两个阶段，即从第二区域的外边界到第一区域的边界的第一阶段，及第一区域的边界到A点的第二阶段，该第一阶段的运动速度为v1，第二阶段的运动速度为v2，此时，v1<v2。

一实施例中，该末端器械在相应方向的运动速度跟末端器械与第二区域的外边界之间的距离正相关，也即，末端器械与第二区域的外边界之间的距离较小时，运动速度较小；末端器械与第二区域的外边界之间的距离较大时，运动速度也较大。通常，当末端器械到达位姿限位的边界且运动方向朝向位姿限位外时，其运动速度基本等于0；而在末端器械到达安全区域的边界且运动方向远离位姿限位时，其运动速度恢复到基本正常。

较佳的，该末端器械在相应方向的运动速度跟末端器械与第二区域的外边界之间的距离呈线性的正相关。较佳的，该末端器械在相应方向的运动速度跟末端器械与第二区域的外边界之间的距离呈指数级的正相关。这样的设计均能使得医生能够显著的感觉到末端器械正在从第二区域的内边界向外边界的边界运动。

其它实施例中，末端器械可以在第一区域内以第一恒定速度运动，并在第二区域以第二恒定速度运动。通常，第一恒定速度大于第二恒定速度。

一些实施例中，末端器械在不同区域及/或不同运动方向的运动速度的变化通常是基于第一部分整体运动速度的变化而发生的改变。例如，通过改变第一部分运动速度的一定比例值而改变末端器械的运动速度。该比例值跟末端器械所处区域以及运动方向相关。

一些实施例中，末端器械在不同区域及/或不同运动方向的运动速度的变化也可以不基于第一部分整体运动速度的变化而发生的改变。例如，在第一部分的自由度相较于期待实现的任务自由度存在足够冗余时，可以解算获得不同区域及/或不同运动方向的末端器械获得不同的运动速度。

一实施例中，运动输入设备是机动式的运动输入设备，其具有多个关节组件、与控制器耦接用于感应各关节组件状态的传感器及与控制器耦接用于驱动各关节组件运动的驱动电机。在该结构基础上，如图18所示，上述控制第一部分运动的步骤还可以包括：

步骤S601’，获取配置的位于位姿限位内的安全运动区域。

为描述方便，在步骤S601’中，同样基于位姿限位和安全运动区域同样划分为前文所描述的第一区域和第二区域。

步骤S602’，根据末端器械在第一区域及第二区域内位置及运动方向的变化而改变运动输入设备的阻力。

其中，步骤S602’主要根据阻力使关联方向上的驱动电机产生反向力矩。在该步骤S602’中，即根据末端器械在第一区域及第二区域内位置及运动方向的变化而改变运动输入设备的阻力的步骤具体可以这样来实现：

例如，在末端器械从第一区域的边界向第二区域的外边界运动时，增大运动输入设备在相应方向移动时的阻力；而在末端器械从第二区域的外边界向第一区域的边界运动时，减小运动输入设备在相应方向移动时的阻力。

一实施例中，运动输入设备在相应方向移动时的阻力跟末端器械与第二区域的外边界之间的距离负相关。通常，当末端器械到达位姿限位的边界且运动方向朝向位姿限位外时，其运动速度基本等于0，此时，医生操作运动输入设备的阻力将极大，在较高的阻力下，运动输入设备几乎不能被医生移动，可以使末端器械运动速度趋近于0；而在末端器械到达安全区域的边界且运动方向远离位姿限位时，其运动速度恢复到基本正常。

较佳的，运动输入设备在相应方向移动时的阻力跟末端器械与第二区域的外边界之间的距离呈线性的负相关。较佳的，运动输入设备在相应方向移动时的阻力跟末端器械与第二区域的外边界之间的距离呈指数级的负相关。这样的设计同样能使得医生能够显著的感觉到末端器械正在从第二区域的内边界向外边界的边界运动，并且，可以实现良好的力反馈。

其它实施例中，末端器械在第一区域运动时，运动输入设备在相应方向移动时的阻力为第一恒定阻力，而末端器械在第二区域运动时，运动输入设备在相应方向移动时的阻力为第二恒定阻力。通常，第二恒定阻力大于第一恒定阻力。

一实施例中，提供一种计算机可读存储介质，该计算机可读存储介质存储有计算机程序，计算机程序被配置为由处理器加载并执行实现如下步骤：获取末端器械的当前位置，并初始化目标位置和指令位置为当前位置，目标位置为期望末端器械达到的位置，指令位置为末端器械实际执行的位置；获取末端器械的增量位置；根据增量位置更新目标位置；在目标位置没有达到末端器械的位置限位时，更新指令位置为目标位置，并计算相邻时刻指令位置之间的位置偏差；在位置偏差达到位置偏差阈值时，修正指令位置获得使位置偏差减小的修正指令位置；控制第一部分运动以使末端器械运动至修正指令位置。

一实施例中，提供一种手术机器人的控制装置。如图19所示，该控制装置可以包括：处理器(processor)501、通信接口(Communications Interface)502、存储器(memory)503、以及通信总线504。

处理器501、通信接口502、以及存储器503通过通信总线504完成相互间的通信。

通信接口502，用于与其它设备比如各类传感器或电机或电磁阀或其它客户端或服务器等的网元通信。

处理器501，用于执行程序505，具体可以执行上述方法实施例中的相关步骤。

具体地，程序505可以包括程序代码，该程序代码包括计算机操作指令。

处理器505可能是中央处理器CPU，或者是特定集成电路ASIC(ApplicationSpecific Integrated Circuit)，或者是被配置成实施本发明实施例的一个或多个集成电路，或者是图形处理器GPU(Graphics Processing Unit)。控制装置包括的一个或多个处理器，可以是同一类型的处理器，如一个或多个CPU，或者，一个或多个GPU；也可以是不同类型的处理器，如一个或多个CPU以及一个或多个GPU。

存储器503，用于存放程序505。存储器503可能包含高速RAM存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。

程序505具体可以用于使得处理器501执行以下操作：获取末端器械的当前位置，并初始化目标位置和指令位置为当前位置，目标位置为期望末端器械达到的位置，指令位置为末端器械实际执行的位置；获取末端器械的增量位置；根据增量位置更新目标位置；在目标位置没有达到末端器械的位置限位时，更新指令位置为目标位置，并计算相邻时刻指令位置之间的位置偏差；在位置偏差达到位置偏差阈值时，修正指令位置获得使位置偏差减小的修正指令位置；控制第一部分运动以使末端器械运动至修正指令位置。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种手术机器人的控制方法，所述手术机器人包括驱动臂以及操纵所述驱动臂中第一部分远端位姿变化的运动输入设备，所述第一部分远端具有末端器械，其特征在于，所述控制方法包括如下步骤：

获取所述末端器械的当前位置，并初始化目标位置和指令位置为所述当前位置，所述目标位置为期望所述末端器械达到的位置，所述指令位置为所述末端器械实际执行的位置；

获取所述末端器械的增量位置；

根据所述增量位置更新所述目标位置；

在更新后的所述目标位置没有达到所述末端器械的位置限位时，更新所述指令位置为更新后的所述目标位置，并计算相邻时刻所述指令位置之间的位置偏差；

在所述位置偏差达到位置偏差阈值时，修正更新后的所述指令位置获得使所述位置偏差减小的修正指令位置；

控制所述第一部分运动以使所述末端器械运动至所述修正指令位置。

2.根据权利要求1所述的控制方法，其特征在于，所述控制方法还包括：

在更新后的所述目标位置达到所述末端器械的位置限位时，停止更新所述指令位置。

3.根据权利要求1所述的控制方法，其特征在于，所述控制方法还包括：

在所述位置偏差低于所述位置偏差阈值时，控制所述第一部分运动以使所述末端器械运动至更新后的所述指令位置。

4.根据权利要求1所述的控制方法，其特征在于，修正更新后的所述指令位置获得使所述位置偏差减小的修正指令位置的步骤具体为：

根据所述位置偏差的一定比例值对更新前的所述指令位置及/或更新后的所述目标位置进行修正从而获得所述修正指令位置。

5.根据权利要求1所述的控制方法，其特征在于，修正更新后的所述指令位置获得使所述位置偏差减小的修正指令位置的步骤包括：

获取所述末端器械在更新前的所述指令位置的第一运动状态及在更新后的所述目标位置的第二运动状态；

根据所述第一运动状态及所述第二运动状态规划所述末端器械的第一运动轨迹；

对所述第一运动轨迹进行时间分割插补获得所述修正指令位置。

6.根据权利要求5所述的控制方法，其特征在于，所述第一运动状态包括所述末端器械在更新前的所述指令位置的位置、速度及加速度，所述第二运动状态包括所述末端器械在更新后的所述目标位置的位置、速度及加速度。

7.根据权利要求5所述的控制方法，其特征在于，所述第一运动轨迹是S曲线、三次曲线或五次曲线。

8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被配置为由处理器加载并执行实现如权利要求1～7任一项所述的控制方法的步骤。

9.一种手术机器人的控制装置，其特征在于，包括：

存储器，用于存储计算机程序；

及处理器，用于加载并执行所述计算机程序；

其中，所述计算机程序被配置为由所述处理器加载并执行实现如权利要求1～7任一项所述的控制方法的步骤。

10.一种手术机器人，其特征在于，包括：

驱动臂；

运动输入设备，所述运动输入设备操纵所述驱动臂中第一部分远端设置的末端器械的位姿变化；

以及控制器，所述控制器与所述驱动臂及所述运动输入设备耦接，并被配置成执行如权利要求1～7任一项所述的控制方法的步骤。

Images