CN117813064A - 用于实现机器人器械对被动控制器的跟踪的一种装置、计算机实现的方法和计算机程序 - Google Patents

Abstract

一种装置，包括：至少一个处理器；以及包括计算机程序代码的至少一个存储器，至少一个存储器和计算机程序代码配置成利用至少一个处理器来导致该装置：从被动控制器接收控制命令，被动控制器配置成远程控制具有多个关节和末端执行器的机器人器械，控制命令定义末端执行器的期望姿态；基于控制命令而使用逆向运动学算法来确定多个关节中每个的关节参数，以使末端执行器能够在受约束环境内尽可能接近地模拟期望姿态，其中逆向运动学算法配置成：使用第一关节参数集作为初始条件来运算末端执行器的姿态；将运算出的姿态与期望姿态比较以确定跟踪误差；针对一个或多个不同的关节参数集重新迭代运算步骤和比较步骤；以及，仅在该跟踪误差小于与末端执行器的当前姿态相关联的跟踪误差的情况下，才将具有最小跟踪误差的运算出的姿态识别为全局解，否则将当前姿态识别为全局解；并且其中，该装置进一步配置成：如果具有最小跟踪误差的运算出的姿态被识别为全局解，则生成马达命令，以根据与全局解相关联的关节参数集而重新配置多个关节。

Description

用于实现机器人器械对被动控制器的跟踪的一种装置、计算 机实现的方法和计算机程序

技术领域

本发明总体上涉及远程控制的机器人系统，且特别是用于实现机器人器械对被动控制器的跟踪的一种装置、相关联的方法和计算机程序。

背景技术

远程控制的机器人系统可用于多种应用，特别是人体进入、安全或两者都受限制的应用。例如，远程控制的机器人系统可用于微创手术，其中进入待手术部位的通道被限制于自然腔道和/或小切口。人手太大而无法进入这样的区域，因此可以改为使用由外科医生远程控制的小型机器人。远程控制的机器人系统还用于可以从安全距离远程操作机器人的军事应用中，诸如炸弹处置。

自此，本发明主要关于手术机器人系统来描述。然而这仅仅为了示范性目的，并不排除本发明在其它领域中的应用。

已知的远程控制手术机器人系统包括控制器和机器人器械，其中用户可以通过操纵控制器来向机器人器械发出命令。已知的控制器包括基座、耦合到基座并包括多个关节(例如可旋转关节或棱柱关节)的可铰接臂、以及耦合到可铰接臂并可通过关节运动相对于基座移动的手柄。已知的机器人器械包括基座、耦合到基座并包括多个关节的可促动臂、以及耦合到可促动臂并可通过关节运动相对于基座促动的末端执行器。

控制器和机器人器械(特别是手柄和末端执行器)均具有在相应的控制工作空间和器械工作空间内的移动自由度。而且，手柄和末端执行器可各自具有在其相应工作空间中的六个移动自由度，包括沿着三个轴线的平移移动和围绕三个轴线的旋转移动。手柄和末端执行器可以被认为具有取决于平移移动的在相应工作空间中的位置，以及取决于旋转移动的相对于相应工作空间的取向。而且，手柄或末端执行器的位置和取向可以共同被视为手柄或末端执行器的姿态。

用户可以操纵手柄在控制器工作空间内的位置和取向，并且可以经由测量可铰接臂中每个关节的运动来跟踪该操纵。这些操纵可以转换成用于机器人器械(特别是可促动臂的关节)的命令以进行促动，使得末端执行器的位置和取向与控制器工作空间中手柄的操纵对应地在器械工作空间中移动。

已知的控制器是主动控制器，这意味着当正在使用机器人系统时，旋转控制器中的每个关节所要求的扭矩取决于来自用户的输入而主动地变化。例如，如果用户要松开控制器，则每个关节中的扭矩可能变化，使得手柄被保持在用户将其操纵到的最后位置和取向。换言之，除非用户提供进一步的输入，否则控制器会冻结。推而广之，机器人器械也会冻结。

而且，在机器人器械的移动受其可用工作空间限制的实例中，可以通过例如限制关节超过某点的旋转来对控制器实施对应的限制。这防止控制器变得与机器人器械错位。

然而，主动地改变控制器中每个关节的扭矩，要求控制器包括昂贵且庞大的构件，诸如伺服马达。因此，已知的主动控制器制造昂贵且缺乏便携性。而且，控制器的关节中扭矩的主动变化可能促使用户朝向不希望和/或不自然的位置，其可能使用户感到沮丧或导致错误产生。

在本说明书中列出或讨论先前发表的文献或任何背景，不应当被必然视为承认该文献或背景是现有技术的一部分或是公知常识。本公开的一个或多个方面/实施例可以解决或不解决背景问题中的一个或多个。

发明内容

根据本发明的第一方面，提供了一种装置，该装置包括：至少一个处理器；以及包括计算机程序代码的至少一个存储器，至少一个存储器和计算机程序代码配置成利用至少一个处理器来导致该装置：从被动控制器接收控制命令，被动控制器配置成远程控制具有多个关节和末端执行器的机器人器械，控制命令定义末端执行器的期望姿态；基于控制命令而使用逆向运动学(IK)算法来确定多个关节中每个的关节参数，以使末端执行器能够在受约束环境内尽可能接近地模拟期望姿态，其中IK算法配置成：使用第一关节参数集作为初始条件来运算末端执行器的姿态；将运算出的姿态与期望姿态比较以确定跟踪误差；针对一个或多个不同的关节参数集而重新迭代运算步骤和比较步骤；以及仅在该跟踪误差小于与末端执行器的当前姿态相关联的跟踪误差的情况下，才将具有最小跟踪误差的运算出的姿态识别为全局解，否则将当前姿态识别为全局解；并且其中，该装置进一步配置成：如果具有最小跟踪误差的运算出的姿态被识别为全局解，则生成马达命令，以根据与全局解相关联的关节参数集而重新配置多个关节。

被动控制器是包括可自由移动的关节的控制器，旋转关节所需的扭矩不会像主动控制器中那样发生主动变化。由于不要求诸如伺服马达的昂贵、笨重和/或庞大的构件，被动控制器可以比主动控制器制造更便宜、更小且更轻。而且，由于不存在关节扭矩的主动变化，因此不会使用户的移动偏向不舒服或不自然的姿态，其可能导致向关联的机器人器械传输次优或意外的命令。

然而，由于被动控制器的用户自由地操纵被动控制器而不受人为限制，因而有可能将被动控制器移动到相关联的机器人器械不能精确地模拟的姿态(位置和取向)。例如，基于机器人器械的形状和关节构型，机器人器械可以被限制于在受约束环境内移动，并且被动控制器可以被移动到机器人器械不能在受约束环境的限制内复制的姿态。这可能起因于例如被动控制器与机器人器械之间的形状和构型的差异。而且，例如出于安全原因，机器人器械能够移动的速度可能是有限的，并且被动控制器的用户可能太快地移动被动控制器而使机器人器械不能镜像。

在使用中，装置的用户可以移动被动控制器，以导致发出控制命令，从而设定机器人器械的末端执行器的期望姿态。然后，装置使用IK算法来确定机器人器械的关节参数以模拟期望姿态。特别地，IK算法运算末端执行器的一个或多个姿态，其中每个运算出的姿态基于作为初始条件的相应的关节参数集，并且与期望姿态比较，以确定运算出的姿态的跟踪误差，即运算出的姿态模拟期望姿态的准确程度。然后，IK算法将具有最小跟踪误差的运算出的姿态识别为全局解，除非最小跟踪误差大于与末端执行器的当前姿态相关联的跟踪误差，在此情况下，当前姿态被识别为全局解。换言之，如果运算出的姿态比当前姿态更准确，则选择最准确地模拟期望姿态的运算出的姿态作为全局解。如果具有最小跟踪误差的运算出的姿态被识别为全局解，而非当前姿态，则该装置将生成马达命令，以根据全局解而移动机器人器械。

借助于本发明，机器人器械可以被控制以尽可能接近地跟踪被动控制器，即，具有可用于通过IK算法运算的姿态的最低可能的跟踪误差。而且，由于具有最小跟踪误差的运算出的姿态总是与实现之前的当前姿态比较，因而该装置绝不会将末端执行器移动到比当前姿态更差的姿态。

在本发明的实施例中，可以随机地选择不同的关节参数集。

在本发明的这种实施例中，IK算法在初始条件下实现随机射击法。这使得全局解能够使用局部算法而找到。

在本发明的实施例中，逆向运动学算法可以配置成重新迭代运算步骤和比较步骤，直到预定时间限制已到期或直到已满足预定终端约束。

在本发明的这种实施例中，终端约束指示IK算法的预期准确度，即将被认为精确的解的期望姿态与运算出的姿态之间的最大跟踪误差。这被要求来说明数字计算的数值准确度、器械的实际精度，以及确保有限收敛时间。

在本发明的实施例中，逆向运动学算法可以包括预定位置限制，以能够运算满足机器人器械的位置约束的末端执行器的部分受约束姿态。

在本发明的这种实施例中，位置约束可以基于机器人器械的物理限制。因此，部分受约束姿态的运算确保该姿态在物理上是可能的。

在本发明的实施例中，逆向运动学算法还可以包括预定速度限制，以能够运算满足机器人器械的位置约束和速度约束两者的末端执行器的受约束姿态。

在本发明的这样的实施例中，预定速度限制可以基于被视为安全的机器人器械的最大速度。将机器人器械的控制限制于受约束姿态可以允许用户监视机器人器械的移动、测定其轨迹，并且能在该移动可能不安全的情况下取消该移动。

在本发明的实施例中，逆向运动学算法可以配置成分开地应用预定位置限制和预定速度限制，以实现末端执行器的受约束姿态和部分受约束姿态两者的运算。

在本发明的这种实施例中，用户可能能够选择性地允许该装置基于部分受约束姿态而非受约束姿态来控制机器人器械。这可以有助于克服其中将机器人器械限制于受约束姿态过度地限制了机器人器械跟踪被动控制器的能力的情形。

在本发明的实施例中，被动控制器和机器人器械可以具有在相应的控制工作空间和器械工作空间内的移动自由度，并且控制工作空间可以被映射到器械工作空间，以允许当被动控制器在控制工作空间内移动时，机器人器械的姿态跟踪被动控制器的姿态。而且，被动控制器可以包括解锁机构，解锁机构配置成继跟踪中断之后重启启动机器人器械姿态到被动控制器姿态的跟踪，并且解锁机构的激活可以导致该装置：确定被识别为受约束全局解的受约束姿态是否匹配被识别为部分受约束全局解的部分受约束姿态；以及如果受约束姿态和部分受约束姿态不匹配，则根据与部分受约束全局解相关联的关节参数集而生成马达命令。

用户也许有可能在控制工作空间中以导致不能被在器械工作空间内移动的末端执行器完全相同地复制的方式移动被动控制器。例如，当被动控制器的运动映射到比最大关节-空间速度限制更快的机器人器械运动时，机器人器械可能冻结。

解锁机构的激活导致装置检查机器人器械的当前受约束姿态是否等同于被识别为部分受约束全局解(即机器人器械的位置限制内的最佳可用解)的部分受约束姿态。若非如此，则装置会将机器人器械移动到部分受约束姿态。因此，在其中跟踪可能已被中断的实例中，解锁机构允许机器人器械恢复被动控制器的优化跟踪。

在本发明的实施例中，如果部分受约束姿态不满足机器人器械的速度约束，则该装置可以配置成：使用轨迹算法来确定机器人器械在器械工作空间内的移动轨迹，使得末端执行器在预定速度限制内从当前姿态移动到部分受约束姿态；以及生成马达命令，以根据所确定的轨迹而自动地控制机器人器械的移动。

在本发明的这种实施例中，轨迹算法的使用确保在解锁机构被激活时，机器人器械可以移动到部分受约束姿态，同时仍然满足速度限制。

在本发明的实施例中，该装置可以配置成当被动控制器的姿态改变时，重新确定移动轨迹。

在本发明的这种实施例中，如果当机器人器械正跟随所确定的轨迹时用户移动被动控制器，则将相应地重新确定轨迹，以确保机器人器械以针对当前被动控制器姿态而非先前姿态运算的部分受约束姿态结束其轨迹。

在本发明的实施例中，轨迹算法可以包括全局缩放因子，全局缩放因子被计算以确保机器人器械的移动不超过预定速度限制。

在本发明的这种实施例中，当机器人器械处于正常操作下时，即当解锁机构未被激活时，应用全局缩放因子可以将机器人器械的关节在机器人器械跟随所确定轨迹的同时移动的速度相对于将针对相同轨迹而要求关节移动的速度减小。

在本发明的实施例中，轨迹算法可以包括用以将机器人器械的移动减速到低于预定速度限制的用户定义的缩放因子。

在本发明的这种实施例中，用户定义的缩放因子可以比全局缩放因子甚至更大程度地减小关节移动的速度。这可以改进用户在其中该移动可能不安全的实例中对所确定的轨迹进行监视、测定以及可能取消的能力。

在本发明的实施例中，逆向运动学算法可以包括运动学和动力学库解算器、改进的运动学和动力学库解算器、以及顺序二次规划解算器中的一个或多个。而且在本发明的一些实施例中，逆向运动学算法可以包括两个或更多个解算器，该两个或更多个解算器并行地运行，使得识别全局解的解算器首先终止其它解算器。

在本发明的这种实施例中，可以缩短运算解的时间。

在本发明的实施例中，多个关节中每个的关节参数可以包括关节的位置、角度、取向、构型以及速度中的一个或多个。

在本发明的实施例中，逆向运动学算法可以具有如下的形式：

q_init＝q

(p^*,q^*)＝(p,q)

当t<t_max且‖p_ref-p^*‖>ε时：

q_i＝解算器(q_init,p_ref)

选择新的随机q_init

其中，表示从被动控制器接收的期望的末端执行器姿态， 代表通过该算法的当前迭代来计算的末端执行器姿态，指代算法初始条件，其中是基于机器人器械的当前状态(或如果不可得到关节角度传感器反馈，则发送到驱动机器人器械的马达的最后的关节-空间命令)而计算的末端执行器姿态，表示当前控制候选(即迄今为止运算的最佳解)，并且指代接受的算法容差；和表示该算法能够运行的当前时间和最大时间。

在本发明的实施例中，逆向运动学算法可以具有如下的形式：

q_init＝q

(p^*,q^*)＝(p,q)

当t<t_max且‖p_ref-p^*‖>ε时：

q_i＝启动局部算法(q_init,p_ref)

当局部算法正在运行时：

q_i＝分步局部算法(q_init,p_ref)

选择新的随机q_init

其中，标记处于速度限制内的变量，并且局部算法是指计算局部解的任何算法。

在本发明的实施例中，轨迹算法可以具有如下的形式：

k_L＝1

如果s<k_LΔ:→k_L＝s

q_u＝q+k_LΔq^*

如果k_L＝1∶→t＝t+Δt

如果k_L＝1且

其中，表示用以确保所生成的轨迹处于关节空间速度限制内的全局缩放因子，代表用以将该移动减慢到低于预定速度限制的速度的用户定义的缩放因子，并且是用以结束轨迹的终端约束阈值。

在本发明的实施例中，机器人器械可以是手术机器人。

在本发明的实施例中，装置可以包括被动控制器和/或机器人器械。

根据本发明的第二方面，提供了一种计算机实现的方法，该计算机实现的方法包括：从被动控制器接收控制命令，被动控制器配置成远程控制具有多个关节和末端执行器的机器人器械，控制命令定义末端执行器的期望姿态；基于控制命令而使用逆向运动学算法确定多个关节中每个的关节参数，以使末端执行器能够在受约束环境内尽可能接近地模拟期望姿态，其中逆向运动学算法配置成：使用第一关节参数集作为初始条件来运算末端执行器的姿态；将运算出的姿态与期望姿态比较以确定跟踪误差；针对一个或多个不同的关节参数集而重新迭代运算步骤和比较步骤；以及仅在该跟踪误差小于与末端执行器的当前姿态相关联的跟踪误差的情况下，才将具有最小跟踪误差的运算出的姿态识别为全局解，否则将当前姿态识别为全局解；并且其中，该方法还包括：如果具有最小跟踪误差的运算出的姿态被识别为全局解，则生成马达命令，以根据与全局解相关联的关节参数集而重新配置多个关节。

根据本发明的第三方面，提供了一种计算机程序，该计算机程序包括配置成执行根据本发明的第二方面的方法的计算机代码。

本公开内容包括一个或多个相应的方面、示例性实施例或特征，这些方面、示例性实施例或特征可以是单独的，也可以是各种组合，无论是否以该组合或单独的方式具体说明(包括要求保护)。用于执行所讨论的一个或多个功能的相应措施也在本公开内容的范围之内。

上文的概述仅仅是示例性的，而非限制性的。

附图说明

现在将参考附图而仅通过示例的方式描述本发明的实施例，其中：

图1是根据本发明的第一方面的装置在使用中的示意图；

图2是形成图1中所示出的装置的一部分的被动控制器的示意图；

图3是形成图1中所示出的装置的一部分的机器人器械的示意图；

图4a和图4b是图3中所示出的机器人器械的进一步的示意图；

图5是任意函数的全局解的图形表示；

图6是图5中所示出的任意函数的局部解的图形表示；

图7、图8及图9是不同的任意函数的受约束解和无约束解的图形表示；

图10是算法逻辑的示意图；

图11是示范关于图5中所示出的任意函数的特定场景的图形表示；

图12是从图10中所示出的逻辑开发的算法逻辑的示意图；

图13是定位成不同构型的简单的机器人器械的示意图；以及

图14是从图12中所示出的逻辑开发的算法逻辑的示意图。

具体实施方式

首先参考图1，根据本发明的第一方面的装置大体上由参考标号2标示。装置2包括至少一个处理器和至少一个包括计算机程序代码的存储器。装置2被示出为由用户4在手术室6中使用，以对患者8实施外科手术。因此，用户4可以是外科医生。

在本发明的该实施例中，装置2还包括控制站10，控制站10包括一对被动控制器12(图2中更详细地示出)、视窗14以及脚踏控制器16。被动控制器12可以各自由用户4在相应的控制工作空间内操纵。

装置还包括电子显示屏18和手术机器人20，手术机器人20包括一对机器人器械22(图3中更详细地示出)、机器人平台24以及机器人马达26。机器人器械安装到机器人平台24，该平台布置成将机器人器械定位在患者8体内。机器人马达26也安装到机器人平台24，并且操作性地耦合到机器人器械，以驱动机器人器械在相应器械工作空间内的移动。

视窗14、电子显示屏18或两者可以配置成显示器械工作空间和控制工作空间的表示。而且，装置2配置成控制视窗14和/或电子显示屏18，使得机器人器械和被动控制器12的当前位置和/或取向在相应的器械工作空间和控制工作空间的表示内被指示。而且，装置2配置成控制视窗14和/或电子显示屏18，使得当前位置和/或取向以二维或三维指示。

被动控制器12和机器人器械的当前位置和/或取向分别是被动控制器12和机器人器械对装置的最后已知的位置和/或取向。

内窥镜28也安装到机器人平台24并插入患者9体内。内窥镜28可以记录外科手术部位的图像，包括使用中的机器人器械，并且所记录的图像可以传送到控制站10，以在视窗14和/或电子显示屏18中显示，以与控制工作空间和器械工作空间的表示一起示出，或者将工作空间叠加到所记录的图像上。

现在参考图2，每个被动控制器12包括：控制器基座30；可铰接臂32，其耦合到控制器基座30并包括多个控制器关节34；以及手柄36，其耦合到可铰接臂32并包括夹持器38。每个控制器关节34可自由地旋转，即，没办法像已知的主动控制器中那样主动地改变旋转每个关节所要求的扭矩。由此，手柄36可由用户4在相应的控制工作空间内自由地移动(图1中示出)。而且，每个被动控制器12特别是每个手柄36可以被认为具有在相应控制工作空间中的位置和相对于控制工作空间的取向。被动控制器12的位置和取向可以共同被视为被动控制器姿态。

从此开始，关于单个被动控制器12和对应的机器人器械而描述本发明的实施例。然而将理解的是，该装置可以包括两个或更多个被动控制器和对应数量的机器人器械。

现在参考图3，与在图1中定位在患者8体内的机器人器械等同的机器人器械22包括器械基座40、可促动臂42以及末端执行器46。

在本发明的该实施例中，器械基座40是轴，其可以从图1所示的机器人平台24延伸并促进可促动臂42和末端执行器46相对于患者8的合适定位。可促动臂42耦合到器械基座40并包括多个器械关节44。每个器械关节44都是可旋转的，并且旋转可以由形成图1所示的机器人马达26一部分的马达经由缆线来驱动，该缆线从器械平台24延伸穿过器械基座40并附接到相关的器械关节44。末端执行器46耦合到可促动臂42，并且可通过一个或多个器械关节44的旋转而在器械工作空间内移动。机器人器械22特别是末端执行器46可以被认为具有在器械工作空间中的位置和相对于器械工作空间的取向。末端执行器46的位置和取向可以共同被视为机器人器械/末端执行器姿态。

末端执行器46还包括可在张开构型与闭合构型之间移动的一对夹钳48(图3中示出)。夹钳48的张开和闭合可以由用户4通过操纵图2所示的对应被动控制器12的夹持器38来控制。

现在参考图2和图3，每个被动控制器12及其相应的机器人器械22具有在相应的控制工作空间和器械工作空间内的移动自由度。在使用中，控制工作空间被映射到器械工作空间，以允许当被动控制器12在控制工作空间内移动时，机器人器械22的姿态跟踪被动控制器12的姿态。

然而，末端执行器46可在其内移动的器械工作空间典型地与手柄36可在其内移动的控制工作空间在尺度上截然不同。而且，器械关节44在可促动臂42中的布置典型地与控制器关节34在可铰接臂32中的布置截然不同，这意味着器械工作空间的形状通常不同于控制工作空间的形状。

因此，由于上文描述的差异，控制工作空间不能始终被完全地映射到器械工作空间。这意味着用户4有可能将手柄36移动到控制工作空间中的某个姿态，该姿态不能由在器械工作空间内移动的末端执行器46完全相同地复制。

现在参考图4a和图4b，机器人器械22以八个器械关节具有六个自由度(DoF)。具体的器械关节在表1中进一步定义。

表1：机器人器械关节的定义

参考符号	关节类型	关节名称	注释
				100	虚拟	基座	这是位于机器人基座处的虚拟关节
101	回转	滚转(roll)	该关节提供器械的滚转运动
				102	棱柱	平移	该关节提供沿着Z轴线的轴向平移
103、106	回转	肘俯仰(pitch)	这两个关节是成对的
				104、105	回转	肘偏航(yaw)	这两个关节是成对的
107	回转	腕俯仰	该关节用于腕俯仰
				108	回转	腕偏航	该关节用于腕偏航
109、110	虚拟	夹钳	两个关节被耦合并提供夹钳张开

基座关节100是指示机器人基座位于何处的虚拟关节。滚转关节101是提供滚转运动的回转关节。平移关节102是提供平移运动的棱柱关节。肘俯仰关节103、106是用于肘俯仰运动的两个成对的关节，它们的移动是相同的。类似地，肘偏航关节104、105是用于肘偏航运动的两个成对的关节，它们的移动是相同的。腕俯仰关节107是用于腕俯仰运动的回转关节，而腕偏航关节108是用于腕偏航运动的回转关节。最后，夹钳关节109和关节110分别是用于左夹钳和右夹钳的两个虚拟关节。

在下文中陈述逆向运动学算法的描述，逆向运动学算法被本发明的实施例使用以确定如下的马达命令：对器械关节101-108进行促动，以便即使精确解是不可能的，机器人器械22的姿态也尽可能准确地跟踪相关联的被动控制器32(图2中示出)的姿态。

逆向运动学-基础

机器人器械的末端执行器姿态可指代为x，针对该特定末端执行器姿态所要求的关节角度集可指代为q。在形式上，雅可比行列式是一组偏微分方程：

其能够改写成：

实质上，雅可比行列式涉及q元素的移动如何导致x元素的移动。方程(2)可以改写为：

其中，是关节的速度，是末端执行器的速度。IK问题运算在给定期望的末端执行器扭转时的期望关节速度，即末端执行器在其以高达六个自由度朝向期望姿态行进时的速度。

表2：关节坐标之间的变换

耦合矩阵

为了实现成对关节的相同运动，使用耦合矩阵。耦合矩阵C将独立的关节构型映射到耦合的关节构型q：

对于机器人器械22，该耦合矩阵被定义为8×6矩阵：

该耦合矩阵还能够用于将针对q的雅可比行列式映射到

其中，J是对于q的6×8雅可比行列式，是对于q^的6×6雅可比行列式：

正向运动学

正向运动学基于输入关节角度而运算末端执行器姿态。从第一关节起，它逐个关节地通过运动学链并获得末端执行器相对于通常位于第一关节处的基础坐标的姿态。其实现方式可从KDL库(Orocos Project https://www.orocos.org/kdl.html)“KDL::ChainFkSolverPos_recursive::JntToCart()”得到。

为了在正向运动学中处置成对的关节，使用耦合矩阵。其实现方式可在“pr::ChainFkSolverPosCoupling_recursive”中得到。

雅可比行列式的计算

雅可比行列式基于当前的关节构型来运算。其实现方式在“ChainJntToJacSolverCoupling::JntToJacCoupling()”中。

解算器的类型

数值IK算法能分成两个子集，即全局优化算法和局部优化算法。前者着眼于整个搜索空间并提供最佳可能解(全局解)。后者采用数学函数知识并查找接近算法初始条件的最佳解，即进一步运算所基于的初始状态。

可以使用全局算法找到的全局解202的示例在图5中针对任意函数200而示出；它是函数的总体最小值。

局部算法将函数的当前值与其邻域比较，并将解朝向较小的值移动。而且，当所有邻近值都高于当前解时，局部算法终止。图6示出基于两个不同初始条件208、210的相同函数200的两个可能局部解204、206。一个局部解204等同于全局解202(在图5中示出)，而另一个局部解206则不同。

如果当机器人器械22相对于由用户4通过被动控制器12设定的期望末端执行器姿态已处于局部最小量时，装置2(图1中示出)要使用局部算法，则局部算法将保持返回相同的局部解(例如图6中的206)，即使可能有更好的解(例如图6中的204)。结果，将不会执行机器人器械22的运动，且机器人器械22看起来对用户4无响应。

即使全局算法由于它们提供目标最佳答案而可能看起来优于局部算法，局部算法也通常是优选的，因为它们更快，在有限的时间内终止，并具有更好的平滑性。出于本公开的目的，平滑性能够粗略地解释为防止机器人器械22跳跃或剧烈移动。

本发明的实施例通过在局部算法的初始条件上实施随机射击法来找到全局解，同时得益于局部算法的功效。利用随机选择的初始条件多次重启局部算法。最后，仅从该算法返回所找到的最佳局部解。

特别地，装置2配置成从被动控制器12接收控制命令，被动控制器12配置成远程控制具有多个关节44和末端执行器46的机器人器械22，而控制命令定义末端执行器46的期望姿态。然后，装置2基于控制命令而使用逆向运动学算法确定多个关节44中每个的关节参数，以使末端执行器46能够在受约束环境(即，器械工作空间)内尽可能接近地模拟期望姿态。逆向运动学算法配置成使用第一关节参数集作为初始条件来运算末端执行器46的姿态；将运算出的姿态与期望姿态比较以确定跟踪误差；针对一个或多个不同的关节参数集而重新迭代运算步骤和比较步骤；以及，如果该跟踪误差小于与末端执行器46的当前姿态相关联的跟踪误差，则将具有最小跟踪误差的运算出的姿态识别为全局解，否则当前姿态被识别为全局解。如果具有最小跟踪误差的运算出的姿态被识别为全局解，则装置2进一步配置成生成马达命令，以根据与全局解相关联的关节参数集而重新配置多个关节44。

随机地选择不同的关节参数集。因此，针对一个或多个不同的关节参数集而重新迭代运算步骤和比较步骤提供允许找到全局解的随机射击法。

约束

在使用中，装置2特别是机器人器械22受到各种限制。特别地，末端执行器46的位置受限于器械工作空间。而且，末端执行器被允许移动的速度可以基于机器人器械的物理限制或对于特定应用被视为安全的最大速度而受限制。为了说明这些限制，向优化问题添加了约束条件，其将算法限制为可行的解集。在图7中，基于当前状态301而针对任意函数300运算解。可行解被约束302限制于任意函数300的受约束区域304。无约束全局解306存在于受约束区域304之外，因此是不可行的。同时，局部解308存在于受约束区域304内，因此是可行的。还存在可行的受约束全局解310，因为它在受约束区域304内，并且还优于无约束局部解308。

在图8中，基于相同的当前状态301并利用应用于提供受约束区域404的相同约束302来针对新的任意函数400运算解。在该示例中，与图7所示的解类似地存在无约束全局解406，无约束全局解406在受约束区域404之外，因此是不可行的。然而，对于任意函数400存在局部解408，局部解408是受约束区域404中的最佳可用解，因此是受约束全局解。

在图9中，基于相同的当前状态301但利用提供受约束区域504的新约束502来针对另外的任意函数500运算解。与图7和图8中所示的解类似地存在无约束全局解506，无约束全局解506在受约束区域504之外，因此是不可行的。在该实例中，无约束局部解508也存在于受约束区域504之外。然而，受约束局部解510存在于受约束区域504内，受约束局部解510是受约束区域504中的最佳可用解，因此是受约束全局解及受约束局部解。

在本发明的实施例中，IK算法包括预定位置限制，以能够运算满足机器人器械的位置约束的末端执行器的部分受约束姿态。位置约束可以基于器械工作空间，器械工作空间又基于机器人器械22(图3)的物理限制，诸如机器人器械22的长度、可由每个关节44实现的运动范围、以及每个关节44沿着机器人器械22长度的位置。为了确保通过IK算法运算的任何解在物理上是可能的，当正使用装置2时，可以始终应用位置约束，使得所有解都部分地受约束。

IK算法还可以包括预定速度限制，以能够运算满足机器人器械的位置约束和速度约束两者的末端执行器的受约束姿态。预定速度限制可以基于机器人器械的最大速度，该最大速度被视为安全的，并且允许用户监视该移动、测定轨迹，以及在移动可能不安全时能够取消移动。

逆向运动学-基础

在本发明的实施例中，逆向运动学算法包括运动学和运动学库解算器、改进的运动学和运动学库解算器、以及顺序二次规划解算器中的一个或多个。

常规IK–运动学和运动学库(KDL)解算器

存在两种类型的IK：速度和位置。位置IK的运算可以基于速度IK。

对于速度IK，目标是基于末端执行器扭转′x而运算关节速度′q。重新调用：

′q＝J^-1′x。 (8)

由于雅可比行列式可能并非可逆矩阵，因而使用伪逆矩阵。为了估计雅可比行列式的伪逆矩阵J⁺，使用奇异值分解(SVD)。SVD定理规定任何矩阵能够被写为三个非唯一矩阵的乘积。因此，m×n雅可比矩阵能够被因式分解为：

J＝UΣV^T， (9)

其中，∑是具有被称为奇异值的非负对角元素的m×n对角矩阵。U是m×m矩阵，且V是n×n矩阵。通常将SVD写为向量外积的和：

其中，σ_i是在奇异向量中从大到小排序的奇异值。然后，能够通过采取所有非零奇异值的倒数来从SVD找到雅可比行列式的伪逆矩阵。特别地，伪逆矩阵J⁺由下式给出：

其中，r是J的秩。该方法的实现方式可在KDL的“ChainIKSolverVel_pinv”中得到。

对于位置IK，目标是使用末端执行器姿态x来运算关节位置q。这能够通过利用正向运动学(FK)和速度IK解算器来实现。实现方式可在“pr::ChainIKSolverPosCouplingNrJntLimit::CartToJntCoupling”中得到。

并行二次规划IK(TRAC-IK)

上文介绍的已知KDL解算器在当机器人器械尚未达到关节限制(即，由于其物理特性如一个或多个关节所允许的运动范围而导致的移动限制)时的正常条件下优良地工作。根据Beeson、Patrick以及Barrett Ames(TRAC-IK：用于一般逆向运动学的改进求解的开源库…2015IEEE-RAS第15届人形机器人(人形机)国际会议)，KDL具有以下问题：

1.具有关节限制的机器人的频繁收敛失败；

2.当搜索“卡”在局部最小量中时，不采取行动；

3.对笛卡尔姿态容差的支持不足；以及

4.在IK解算器本身中未利用容差。

TRAC-IK提出了KDL解算器的改进和用于具有若干度量的IK的备选非线性优化公式。

首先，代替使用最大迭代次数作为终止约束，改进的解算器使用最大时间。这有助于将KDL解算器与其它IK解算器比较。

其次，改进的KDL解算器通过检查q的当前估计与先前估计之间的差是否接近零来检测局部最小量。当检测到局部最小量时，新的随机q将用于“松开”迭代算法。该改进的KDL解算器的实现方式可在“KDL::ChainIKSolverPos_TL”中得到。

然而，当遇到关节限制时，改进的KDL解算器仍表现出失败。在每次迭代中，KDL解算器将q的值制约于关节限制。因而，关节限制使得搜索空间不平滑，并导致IK解算器在其中存在解的场景下失败。为了减轻该问题，IK问题能够形成为能够使用顺序二次规划(SQP)来局部地求解的非线性优化问题。SQP是用于非线性优化的迭代算法，其能够应用于具有可连续地微分两次的目标函数和约束的问题。目标函数是：

其中，f(q)是返回当前关节构型q的末端执行器姿态的FK解算器，且p是期望姿态。SQP IK方法的实现方式可在“NLOPT_IK::CartToJnt”中得到。

尽管SQP IK解算器在IK求解率的方面胜过KDL解算器，但SQP IK解算器收敛到解的计算时间可能长得多。TRAC_IK提出了一种并行方法，该并行方法并行地运行改进的KDL解算器和SQP解算器，使得在保持计算时间相对低的同时改进了总求解率。并行算法的实现方式可在“TRAC_IK::CartToJnt”中得到，其中两个解算器被启动，并且在允许的时间内首先获得解的解算器能够终止另一个解算器。

因此，在本发明的实施例中，逆向运动学算法包括两个或更多个解算器，该两个或更多个解算器并行地运行，使得识别全局解的解算器首先终止其它解算器。

逆向运动学–本发明的实施例

已在上文描述的TRAC-IK算法中识别关于机器人可控性和人类安全性的两个关键问题。首先，当不可由器械实现期望的主姿态时，器械将会冻结。其次，所提出的IK方案可能导致器械突然地改变运动学构型。为了解决这些问题，已在IK算法中实现以下的修改：

-接受非完美解以防止器械在工作空间边界上冻结；

-添加虚构速度限制以防止构型改变；

-当器械冻结时，提供将器械带回到可行工作空间的解锁动作；以及

-当期望状态在关节-空间速度限制之外时，提供辅助控制模式以生成安全轨迹。

下文更详细地描述对原始算法实现的改变。

接受非完美解以防止器械冻结

在第一IK算法(13)中，通过将所提出的解与期望值比较来检查成功。该算法写为：

q_init＝q

当t<t_max且‖p_ref-p^*‖>ε时：

q_i＝解算器(q_init,p_ref)

选择新的随机q_init

其中，表示从被动控制器接收的期望的末端执行器姿态， 代表通过该算法的当前迭代来计算的末端执行器姿态，指代算法初始条件，其中，是基于机器人器械的当前状态而计算的末端执行器姿态(或如果不可得到关节角度传感器反馈，则发送到驱动机器人器械的马达的最后的关节-空间命令)，表示当前控制候选(即迄今为止运算的最佳解)，并且指代接受的算法容差；和表示该算法能够运行的当前时间和最大时间。如果跟踪误差条件为真，则该算法返回成功，并且与p^*相关联的关节-空间命令发送到驱动机器人器械22的马达。

然而，满足(13)中的跟踪条件的关节-空间值并非始终存在。即使存在可行解，在使用中IK算法也可能不能在专用于控制的时间内找到完美解。在这些情况下，原始算法返回失败，并且器械命令不被更新。结果，器械对主命令无响应，直到找到满足(13)的解。图10示出第一IK算法600的算法逻辑。

对该问题的已知解决方案涉及解决以下的优化问题(或等同问题)。

s.t.q^min≤q^*≤q^max (14)

算法(14)找到最接近期望解的解，而对所找到的解必须接近期望解的程度无任何约束，该期望解将被视为成功的。然而，该解是局部解，因而它不支持如在(13)中使用的随机射击，并且可能卡在局部最小量中。

下文描述的本发明实施例所使用的解采取由(14)(或任何其它局部优化算法，例如KDL或SQP)提供的局部解，并生成在给定时间内找到的全局最佳解。

为了即使在未找到完美解时也保持器械响应于主命令，已修改(13)以提供本发明的实施例所使用的第二IK算法(15)。它写为：

q_init＝q

(p^*,q^*)＝(p,q)

当t<t_max且‖p_ref-p^*‖>ε时：

q_i＝解算器(q_init,p_ref)

选择新的随机q_init。

注意到，与现有技术解决方案(14)相反，IK算法(15)选择关于当前器械姿态(p)的最佳解而非解算器初始条件(p_init＝f(q_init))。这是为了说明TRAC-IK算法中使用的随机初始条件，对于该随机初始条件，所找到的局部解可能比当前机器人器械姿态更远离期望姿态。在图11中关于任意函数200和当前器械状态702而示范该场景的示例。基于随机初始条件704，将找到局部解706，其对应于比当前机器人器械姿态更差的机器人器械姿态。在图12中示出用于第二IK算法(15)的算法逻辑800。

即使在尚未找到精确解时，该算法也返回最佳可用解，而初始条件确保由该算法提出的解不会比机器人器械的当前状态更差。结果，即使在不存在完美解(例如，被动控制器姿态在器械工作空间之外)时，机器人器械也保持响应。

添加虚构速度限制以防止构型改变

有可能对于给定的末端执行器姿态，机器人器械可以具有多于一种可能的构型。例如，图13示出具有第一关节902、第二关节904以及末端执行器906的简单的机器人器械922。在当前构型908下，第一关节902以角度q₁定位，第二关节904以角度q₂定位。然而，对于期望的末端执行器位置p，两种构型是可能的。在第一构型910下，第一关节902将以角度q′₁定位，且第二关节904以角度q′₂916定位。在第二构型912下，第一关节902将以角度q″₁定位，且第二关节904将以角度q″₂定位。

当考虑两种可能的构型时，末端执行器906的位置是等同的，即，

p＝p。 (16)

然而，实现该位置的由机械关节所要求的运动量是不同的，

以单位时间执行的机器人器械的机械关节的运动量被称为关节-空间速度。

在图13中，与从当前构型908移动到第一构型910相比，机器人器械在给定时间量内从当前构型908移动到第二构型912将要求关节902、904的显著更快运动。结果，如果选择了第二构型，则该器械可能看起来跳跃，并且该运动可能是用户感到不可预测的。当大的关节-空间运动对应于末端执行器的位置的微小改变并因而对应于由用户执行的被动控制器的微小运动时，尤其如此。

用以防止构型改变(防止该器械移动到第二构型912而非第一构型910)的标准途径是限制机器人器械的机械关节的允许速度。这些限制被称为关节-空间速度限制。

应用关节-空间速度限制的已知方法是解决以下(或等同)优化问题：

s.t.q_min≤q^*≤q^max

类似地，对于先前的情况，该解决方案仅说明接近算法初始条件的速度限制。此外，当其在每个步骤实施速度限制时，该解决方案防止算法找到全局解。用以说明关节-空间速度限制的常用的备选解决方案写为：

s.t.q^min≤q≤q^max (19)

其中，是加权矩阵。该算法能找到比(18)更广范围的解，但并非严格地强加关节-空间速度限制。通过算法(19)找到使所要求的关节-空间速度和末端执行器跟踪误差相平衡的解。

下文描述的算法利用局部无约束IK算法并在全局级下应用速度限制，而非在局部级下实现速度限制。

为了解决器械构型改变，算法(15)已扩展到说明第三IK算法(20)中的速度限制。它写为：

[1]q_init＝q

[2](p^*,q^*)＝(p,q)

[3]

[4]当t<t_max且时：

[5]q_i＝启动局部算法(q_init,p_ref)

[6]当局部算法正在运行时：

[7]q_i＝分步局部算法(q_init,p_ref)

[8]

[9]

[10]选择新的随机q_init

其中，标记处于速度限制内的变量。“局部算法”是指计算局部解的任何算法。在(20)中，[1]、[2]和[3]表示初始化阶段。在初始化之后，执行迭代循环[4]-[10]，直到满足终端条件。在循环[4]-[10]内，运行用以计算局部解的算法的内部迭代循环[6]-[9]，其中在每次迭代，更新部分受约束解(在预定位置限制内可行)和受约束解(在预定位置限制和预定速度限制两者内可行)。换言之，第三IK算法(20)配置成分开地应用预定位置限制和预定速度限制，以实现末端执行器的受约束姿态和部分受约束姿态两者的运算。

用以计算局部解的算法在本文其余部分中被称为“局部算法”。该局部算法可以是用以计算局部解的任何合适算法，诸如运动学和动力学库解算器、改进的运动学和动力学库解算器、以及顺序二次规划解算器。而且，如在图14中示出的算法逻辑1000所示，两个或更多个解算器可以并行地运行，使得首先识别全局解的解算器终止其它解算器。

初始化阶段始于将局部算法初始条件设定成当前器械状态[1]。然后，部分受约束[2]解和受约束[3]解初始地设置成当前器械状态，以确保通过(20)生成的最终解不会比当前状态更差。

在[4]中，迭代循环启动；当跨越最大时间限制或找到满足终端约束的解时，该迭代循环结束。终端约束指示算法预期准确度，即对于将被认为精确的解，期望状态与当前解(ε)之间的最大跟踪误差。这被要求来说明数字计算的数值准确度、器械的实际精度，以及且确保有限收敛时间。

在循环[4]-[10]的第一次迭代开始时，局部算法始于当前初始状态[5]。在局部算法[6]-[9]的每个步骤，将所找到的当前解独立地与存储的部分受约束([8]中)解和受约束([9]中)解比较。如果当前解比存储的值更优化，并且对于受约束解满足约束，则将存储的值更新成新值。已知的途径以与位置限制相同的方式应用速度限制。然而在本发明的实施例中，在局部算法的每次迭代中检查速度限制，但当跨越速度限制时，不终止局部算法。该途径允许算法除了受约束解(满足两组限制)外还找到部分受约束解(满足位置限制，但不一定满足速度限制)。因此，第三IK算法分开地应用预定位置限制和预定速度限制，以实现末端执行器的受约束姿态和部分受约束姿态两者的运算。

最终，当局部算法终止时，随机地生成新的初始条件，并且循环[4]-[10]的新迭代始于用新初始条件初始化的局部算法。注意，第三IK算法(20)的第一次迭代将始终找到当前状态的局部解，因为局部算法的初始条件与当前状态匹配。仅当尚未局部地找到解时，算法(20)才开始通过改变局部算法的初始条件来查找全局解。重新运行第三IK算法(20)，直到已达到最大时间或已找到满足终端约束的解，即已满足终端条件。换言之，重新迭代[10]运算步骤和比较步骤[5]-[9]，直到预定时间限制已到期或直到已满足预定终端约束[4]。

总体上，第三IK算法(20)是全局优化算法。如果第三IK算法(20)通过终端约束[4]终止，则返回的受约束解和部分受约束解是全局优化的。如果(20)由于时间限制而终止，则不能主张所提供的受约束解的全局优化性。然而，当(20)由于时间限制而终止时，能够通过将终端约束[4]应用于部分受约束解来检查部分受约束解的全局优化性。如果部分受约束解的[4]中的跟踪误差低于给定阈值，则部分受约束解能够被视为全局优化的，否则不能主张其全局优化性。

该途径提供受约束解和部分受约束解两者，并且它支持局部算法的初始条件的随机射击。如下文进一步描述的，默认应用受约束解，而部分受约束解用于在器械运动已由于速度限制而冻结的情况下，解锁器械运动。

提供解锁动作

通过引入速度限制，当被动控制器的运动映射到比最大关节-空间速度限制更快的机器人器械运动时，机器人器械可能冻结。即使对于器械的不同运动学构型，可能存在更好的解，IK算法也可以在器械处于局部最小量的情况下防止器械运动。另外，外科医生可以决定处于给定运动学构型的器械由于系统以速度限制限制允许的器械姿态而感到失控。为了抵消这些影响，被动控制器包括解锁机构，解锁机构配置成继跟踪中断之后，重新启动机器人器械姿态到被动控制器姿态的跟踪。

在本发明的实施例中，可以通过分别按压和释放脚踏控制器16来激活和停用解锁机构，如图1所示，脚踏控制器16操作性地耦合到被动控制器12。在本发明的其它实施例中，被动控制器可以包括用于使用户激活解锁机构的任何合适部件，例如按钮、触发器、杠杆或语音命令系统。

解锁机构的激活导致该装置确定被识别为受约束全局解的受约束姿态是否匹配被识别为部分受约束全局解的部分受约束姿态。如果受约束姿态和部分受约束姿态不匹配，则装置将根据部分受约束全局解生成马达命令。换言之，解锁机构的激活使该装置检查机器人器械的当前受约束姿态是否等同于被识别为部分受约束全局解(即，机器人器械的位置限制内的最佳可用解)的部分受约束姿态。若非如此，则装置会将机器人器械移动到部分受约束姿态。

然而，有可能(若非很可能)部分受约束姿态不满足机器人器械的速度约束。在此情况下，该装置配置成使用轨迹算法来确定机器人器械在器械工作空间内的移动轨迹，使得末端执行器在预定速度限制内从当前姿态移动到部分受约束姿态。然后装置将生成马达命令，以根据所确定的轨迹而自动地控制机器人器械的移动。

在本发明的实施例中，装置将检查当前受约束的机器人器械姿态是否为全局优化的，即装置检查全局无约束解((20)中的p^*)是否匹配受约束解((20)中的)。该检查执行多达最大次数，例如10次。仅当已通过IK算法找到有效解时，重复次数才从最大量减小。如果处在该限制内，则无约束解和受约束解始终匹配，装置返回到正常操作，即解锁机构终止。当受约束解和无约束解不匹配(即)时，应用无约束解。为了在使用无约束解时避免器械的突然非预期运动，使用确保机器人器械在预定速度限制内移动的轨迹算法来确定机器人器械的轨迹。一旦当前机器人器械姿态匹配期望姿态，装置就返回到正常操作。

有可能在解锁机构被激活时，即当机器人器械正自动地移动到部分受约束姿态时，用户可以移动被动控制器。为了确保机器人器械的最终姿态尽可能准确地跟踪当前被动控制器姿态，该装置配置成在被动控制器的姿态改变时，重新确定移动轨迹。

轨迹算法

下文描述本发明的实施例所使用的轨迹算法。

为了将器械设定成不处于器械速度限制内的任意状态，已引入轨迹算法，以将机器人器械从其当前状态q移动到期望状态q*。它写为：

[1]k_L＝1

[2]

[3]

[4]

[5]如果s<k_LΔ:→k_L＝s

[6]q_u＝q+k_LΔq^*

[7]如果k_L＝1:→t＝t+Δt

[8]如果k_L＝1且

其中，表示用以确保所生成的轨迹处于关节空间速度限制内的全局缩放因子，代表用以将移动减慢到低于预定速度限制的速度的用户定义的缩放因子，是用以结束轨迹的终端约束阈值，且q_u是当前控制命令。

轨迹算法(21)始于将全局缩放因子k_L设置成一[1]。该值表示不执行轨迹缩放的情形。然后对于每个器械关节[2]，基于当前(q_k)状态和期望状态生成轨迹中的新步骤[3]。在[4]中，基于跟随轨迹所要求的位移和由最大关节位移(Δq_k,max)表示的关节速度限制来计算当前关节的缩放因子。计算全局缩放因子以确保机器人器械的移动不超过预定速度限制。如果针对(多个)当前关节计算的缩放因子小于当前全局缩放因子(k_L)，即轨迹必须比针对先前关节减慢得更多，则更新全局缩放因子(k_L)[5]。

在[4]的计算中，可以引入额外的用户定义的缩放因子(k_s)。它是由算法的用户在其初始化时定义的可选恒定值。它允许用户将器械运动减慢到低于预定速度限制。该值可用于将自动机器人器械运动减慢到能够由人更容易地监视器械运动的速度。当计算轨迹更新时，全局缩放因子(k_L)应用于所有关节[6]。这是为了确保对于所有关节，轨迹都将在同一时刻结束，其导致更一致的运动。

当应用最终轨迹步骤时，如果不要求缩放，则更新轨迹时间[7]。生成轨迹[3]，因此至多在达到最终点。如果跨越该点，则轨迹将开始减慢，或当时，它将会将器械朝向初始位置移回。当不应用全局缩放因子且期望状态q*恒定时，隐含地满足条件(k_ωt≤π/2)。为了确保当应用缩放因子或在运动生成期间更新期望状态q*时不违反条件(k_ωt≤π/2)，如果要求关节缩放，则由t指示的轨迹生成器进展不会向前移动[7]。

在最后的步骤中，检查终端条件[8]。当未将缩放应用于所生成的轨迹且所有关节都已到达其期望位置一直到给定阈值(k_t)时，认为轨迹结束。终端约束阈值(k_t)是允许算法在有限时间内结束的恒定参数。它说明受控制器械的数值准确度、算法时间步长以及精度。

当正自动地控制机器人器械时，轨迹算法(21)生成命令，以将器械从当前状态(q)安全地移动到期望状态(q^*)。生成的命令被发送到马达。在当前状态达到期望状态(||q-q^*||<ε)时，该算法自动地结束。备选地，当外科医生经由被动控制器的操作停用解锁机构时，该算法立即终止。

在轨迹生成期间，能够安全地更新期望的关节-空间状态(q^*)；在这种情况下，轨迹算法设计确保安全的关节-空间运动。

申请人特此单独地公开本文中描述的每个单独特征和两个或更多个这样特征的任何组合，只要这样的特征或组合能够鉴于本领域技术人员的一般常识而作为整体基于本说明书来实施，而不论这样的特征或特征组合是否解决本文公开的任何问题，并且不限制权利要求的范围。申请人指出，所公开的方面/实施例可以由任何这样的单独特征或特征组合组成。鉴于前文的描述，对于本领域技术人员将明显的是，可以在本公开的范围内作出各种修改。

Claims (22)

1.一种装置，包括：

至少一个处理器；以及

至少一个包括计算机程序代码的存储器，所述至少一个存储器和计算机程序代码配置成利用所述至少一个处理器导致所述装置：

从被动控制器接收控制命令，所述被动控制器配置成远程控制具有多个关节和末端执行器的机器人器械，所述控制命令定义所述末端执行器的期望姿态；

基于所述控制命令而使用逆向运动学算法确定所述多个关节中每个的关节参数，以使所述末端执行器能够在受约束环境内尽可能接近地模拟所述期望姿态，其中所述逆向运动学算法配置成：

使用第一关节参数集作为初始条件来运算所述末端执行器的姿态；

将运算出的姿态与所述期望姿态比较以确定跟踪误差；

针对一个或多个不同的关节参数集重新迭代所述运算步骤和比较步骤；以及

仅在所述跟踪误差小于与所述末端执行器的当前姿态相关联的跟踪误差的情况下，才将具有所述最小跟踪误差的所述运算出的姿态识别为全局解，否则将所述当前姿态识别为所述全局解；

并且其中，所述装置进一步配置成：如果具有所述最小跟踪误差的所述运算出的姿态被识别为所述全局解，则生成马达命令，以根据与所述全局解相关联的所述关节参数集而重新配置所述多个关节。

2.根据权利要求1所述的装置，其中，所述不同的关节参数集被随机地选择。

3.根据权利要求1或2所述的装置，其中，所述逆向运动学算法配置成重新迭代所述运算步骤和比较步骤，直到预定时间限制已到期或直到已满足预定终端约束。

4.根据任一前述权利要求所述的装置，其中，所述逆向运动学算法包括预定位置限制，以能够运算满足所述机器人器械的位置约束的所述末端执行器的部分受约束姿态。

5.根据权利要求4所述的装置，其中，所述逆向运动学算法还包括预定速度限制，以能够运算满足所述机器人器械的位置约束和速度约束两者的所述末端执行器的受约束姿态。

6.根据权利要求5所述的装置，其中，所述逆向运动学算法配置成分开地应用所述预定位置限制和预定速度限制，以能够运算所述末端执行器的受约束姿态和部分受约束姿态两者。

7.根据任一前述权利要求所述的装置，其中，所述被动控制器和机器人器械具有在相应的控制工作空间和器械工作空间内的移动自由度，且其中所述控制工作空间被映射到所述器械工作空间，以允许当所述被动控制器在所述控制工作空间内移动时，所述机器人器械的姿态跟踪所述被动控制器的姿态。

8.根据从属于权利要求6时的权利要求7所述的装置，其中，所述被动控制器包括解锁机构，所述解锁机构配置成继跟踪中断后，重启启动所述机器人器械姿态到所述被动控制器姿态的跟踪，且其中所述解锁机构的激活导致所述装置：

确定被识别为受约束全局解的受约束姿态是否匹配被识别为部分受约束全局解的部分受约束姿态；以及

如果所述受约束姿态和部分受约束姿态不匹配，则根据与所述部分受约束全局解相关联的所述关节参数集而生成马达命令。

9.根据权利要求8所述的装置，其中，如果所述部分受约束姿态不满足所述机器人器械的所述速度约束，则所述装置配置成：

使用轨迹算法确定所述机器人器械在所述器械工作空间内的移动轨迹，使得所述末端执行器在所述预定速度限制内从所述当前姿态移动到所述部分受约束姿态；以及

生成所述马达命令，以根据所确定的轨迹自动地控制所述机器人器械的移动。

10.根据权利要求9所述的装置，其中，所述装置配置成当所述被动控制器的所述姿态改变时，重新确定所述移动轨迹。

11.根据权利要求9或10所述的装置，其中，所述轨迹算法包括全局缩放因子，所述全局缩放因子被计算以确保所述机器人器械的移动不超过所述预定速度限制。

12.根据权利要求9至11中任一项所述的装置，其中，所述轨迹算法包括用户定义的缩放因子，以将所述机器人器械的移动减速到低于所述预定速度限制。

13.根据任一前述权利要求所述的装置，其中，所述逆向运动学算法包括运动学和动力学库解算器、改进的运动学和动力学库解算器、以及顺序二次规划解算器中的一个或多个。

14.根据任一前述权利要求所述的装置，其中，所述逆向运动学算法包括两个或更多个解算器，所述两个或更多个解算器并行地运行，使得首先识别所述全局解的所述解算器终止其它解算器的运行。

15.根据任一前述权利要求所述的装置，其中，所述多个关节中每个的所述关节参数包括所述关节的位置、角度、取向、构型以及速度中的一个或多个。

16.根据任一前述权利要求所述的装置，其中，所述逆向运动学算法具有如下的形式：

q_init＝q

(p^*，q^*)＝(p，q)

当t＜t_max且||p_ref-p^*||＞ε时：

q_i＝解算器(q_init，p_ref)

选择新的随机q_init

其中，表示从所述被动控制器接收的期望的末端执行器姿态， 代表通过所述算法的当前迭代来计算的末端执行器姿态，指代所述算法初始条件，其中是基于所述机器人器械的当前状态或在不可得到关节角度传感器反馈的情况下发送到驱动所述机器人器械的马达的最后关节-空间命令而计算的末端执行器姿态，表示当前控制候选，并且指代所接受的算法容差；和表示所述算法够运行的当前时间和最大时间。

17.根据从属于权利要求6时的权利要求16所述的装置，其中，所述逆向运动学算法具有如下的形式：

q_init＝q

(p^*，q^*)＝(p，q)

当t＜t_max且||p_ref-p^*||＞ε时：

q_i＝启动局部算法(q_init，p_ref)

当局部算法正在运行时：

q_i＝分步局部算法(q_init，p_ref)

选择新的随机q_init

其中，标记处于速度限制内的变量，并且所述局部算法是指计算局部解的任何算法。

18.根据从属于权利要求11时的权利要求12所述的装置，其中，所述轨迹算法具有如下的形式：

k_L＝1

如果s＜k_LΔ：→k_L＝s

q_u＝q+k_LΔq^*

如果k_L＝1：→t＝t+Δt

如果k_L＝1且：-→终止

其中，表示用以确保所生成的轨迹处于所述关节空间速度限制内的全局缩放因子，代表用以将所述移动减慢到低于所述预定速度限制的速度的用户定义的缩放因子，并且是用以结束轨迹的终端约束阈值。

19.根据任一前述权利要求所述的装置，其中，所述机器人器械是手术机器人。

20.根据任一前述权利要求所述的装置，其中，所述装置包括所述被动控制器和/或机器人器械。

21.一种计算机实现的方法，包括：

从被动控制器接收控制命令，所述被动控制器配置成远程控制具有多个关节和末端执行器的机器人器械，所述控制命令定义所述末端执行器的期望姿态；

基于所述控制命令而使用逆向运动学算法确定所述多个关节中每个的关节参数，以使所述末端执行器能够在受约束环境内尽可能接近地模拟所述期望姿态，其中所述逆向运动学算法配置成：

使用第一关节参数集作为初始条件来运算所述末端执行器的姿态；

将运算出的姿态与所述期望姿态比较以确定跟踪误差；

针对一个或多个不同的关节参数集重新迭代所述运算步骤和比较步骤；以及

仅在所述跟踪误差小于与所述末端执行器的当前姿态相关联的跟踪误差的情况下，才将具有所述最小跟踪误差的所述运算出的姿态识别为全局解，否则将所述当前姿态识别为所述全局解；

并且其中，所述方法还包括：如果具有所述最小跟踪误差的所述运算出的姿态被识别为所述全局解，则生成马达命令，以根据与所述全局解相关联的所述关节参数集而重新配置所述多个关节。

22.一种计算机程序，包括配置成执行根据权利要求21所述的方法的计算机代码。