CN114559432A - 手术机械臂自动定位寻路方法、装置、机器人及存储介质 - Google Patents

Abstract

本发明实施例公开了手术机械臂自动定位寻路方法、装置、机器人及存储介质，该方法包括：获取机械臂的当前位姿数据以及手术区域的位置数据；根据所述当前位姿数据和所述位置数据计算最短路径；构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵为满秩，则求解得到所述机械臂的各个关节角度；根据所述各个关节角度控制所述机械臂到达所述手术区域。使得机械臂在确定了路径之后，可以在多个实现完成该路径的解中找到不会使得机械臂产生奇异位的解，保证了手术中患者的安全，提高了机械臂的安全系数。

Description

手术机械臂自动定位寻路方法、装置、机器人及存储介质

技术领域

本发明涉及机械控制领域，尤其涉及一种手术机械臂自动定位寻路方法、装置、机器人及存储介质。

背景技术

在传统手术的牙科植牙过程中，医生手持牙机对植牙部位进行球钻打磨，先锋钻打孔，扩孔钻打孔，以及成型钻打孔等操作。期间需要多次打孔，打孔过程复杂，需要多次依靠医生经验定位患者部位。种植牙入门门槛高，需要经验丰富的医生进行手术。手术机器人自动定位可以解决上述问题。而市面上一般机器人定位方案，上位机规划一个机械臂能够到达的位置，让机械臂以一个姿态进行打钻，由于机械臂为6轴机械臂，在空间中有多个自由度，会存在多解的情况，所以到达的位置不一定为当前位置的最优解。因此可能会以奇异位的姿态到达位置，从而在手术中对患者造成危险。

发明内容

有鉴于此，本申请提供了一种手术机械臂自动定位寻路方法，包括：

获取机械臂的当前位姿数据以及手术区域的位置数据；

根据所述当前位姿数据和所述位置数据计算最短路径；

构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵为满秩，则求解得到所述机械臂的各个关节角度；

根据所述各个关节角度控制所述机械臂到达所述手术区域。

进一步的，还包括：

若所述雅克比矩阵不为满秩，则根据所述手术区域的位置数据计算所述机械臂位于所述手术区域时的所有候选位姿数据，并将所有所述候选位姿数据和所述当前位姿数据进行差值计算；

将计算得到最小差值的候选位姿数据作为选取的位姿数据，根据所述选取的位姿数据和所述最短路径确定所述关节角度。

进一步的，所述机械臂包括前端工具，所述当前位姿数据包括所述前端工具的空间坐标以及机械臂围绕所述前端工具旋转的欧拉角；

将所有所述候选位姿数据和所述当前位姿数据进行差值计算包括：

根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选位姿的四元数坐标，将所述当前位姿的四元数坐标与所述所有候选的目标位姿四元数坐标进行所述差值计算；

根据所述选取的位姿数据和所述最短路径确定所述关节角度包括：

将所述选取的位姿的四元数坐标转换成相应的欧拉角坐标，根据所述选取的位姿的欧拉角坐标和所述最短路径确定所述关节角度。

进一步的，所述根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选的目标位姿数据包括：

根据所述手术区域确定所述前端工具的目标位置，每隔一度，记录所述机械臂顺时针或逆时针围绕所述前端工具在所述目标位置时所形成的所有位姿数据，将所述形成的所有位姿数据作为所述候选的目标位姿数据。

进一步的，所述差值计算的公式为：

Err＝|q_{1_1}-q_{n_1}|+|q_{1_2}-q_{n_2}|+|q_{1_3}-q_{n_3}|+|q_{1_4}-q_{n_4}|

式中，q_{1_1}至q_{1_4}为所述当前位姿数据的四元数数据，q_{n_1}至q_{n_4}为所述所有候选位姿数据的四元数数据，Err为所述差值。

进一步的，所述雅克比矩阵公式为：

q＝J(q)^-1*V_e

式中q为所述机械臂的关节速度，V_e是所述机械臂的末端速度，j(q)^-1表示从关节速度向末端速度映射的雅克比矩阵式的逆。

进一步的，所述最短路径为所述前端工具的空间坐标到所述手术区域的空间坐标连成的直线；

所述最短路径的计算公式为：

p(λ)＝(1-λ)*p₁+λ*p₂

式中p1为所述机械臂前端工具的空间坐标，p2为所述手术区域的空间坐标，λ为斜率参数。

进一步的，本申请实施例还提供一种手术机器人，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行上述实施例中任一项所述的手术机械臂自动定位寻路方法。

进一步的，本申请实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行上述实施例中任一项所述的手术机械臂自动定位寻路方法。

本发明实施例公开了手术机械臂自动定位寻路方法包括：获取机械臂的当前位姿数据以及手术区域的位置数据；根据所述当前位姿数据和所述位置数据计算最短路径；构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵的解为满秩时，确定所述机械臂的各个关节角度，根据关节角度控制所述机械臂到达所述手术区域。使得机械臂在确定了路径之后，可以在多个实现完成该路径的解中找到不会使得机械臂产生奇异位的解，保证了手术中患者的安全，提高了机械臂的安全系数，同时可以在多个解中找到最优的解，使得在手术过程中，效率可以大大提高，给患者更好的治疗体验，也简化了医生的操作，给手术增加容错空间。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1示出了本申请实施例一种手术机械臂自动定位寻路方法流程示意图；

图2示出了本申请实施例又一种手术机械臂自动定位寻路方法流程示意图；

图3示出了本申请实施例机械臂候补姿态示意图；

图4示出了本申请的实施例手术机械臂自动定位寻路装置结构示意图。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

接下来以具体实施例来解释本申请的技术方案。

实施例1

本申请的技术方案应用于牙科手术的手术机械臂，在实际手术过程中，机械臂会携带其末端工具到指定坐标进行手术操作，比如打磨或者切割，为此需要精确控制机械臂的移动，同时也要保障机械臂工作时对于患者来讲是安全的，本实施例以种植手术为例进行解释。

参考图1所示的流程图，本实施例中手术机械臂自动定位寻路方法包括以下步骤：

步骤S100，获取机械臂的当前位姿数据以及手术区域的位置数据；

机械臂的位姿数据包括机械臂末端工具的坐标数据以及机械臂本体的姿态数据，具体可以用欧拉角来表示，如(x，y，z，rx，ry，rz)，来表示机械臂末端工具在空间坐标(x，y，z)时，机械臂围绕末端工具的一个姿态状态，可以理解，上述坐标数据和姿态可以通过机械臂中的相应传感器直接读取或间接计算得到。

手术区域则是根据医生术前规划确定的区域，具体而言为种植体要植入的位置，该术前规划在专门的图像坐标系中进行，并且和机械臂的坐标系进行配准，使得位姿数据以及位置数据处于同一坐标系下。

手术区域为机械臂需要到达的位置，以种植体为例，若术前规划种植体需要垂直的打入牙床，则机械臂末端的打磨工具也需要在同一个位置，以同样的方向，垂直的去打磨牙床骨质，也就是说在确定了手术区域后，也确定了机械臂末端工具在手术区域的一个位置状态，即相对于牙床骨质是垂直还是成一定角度倾斜等。但是对于机械臂如何达到这个位置，却可能存在多种路径方案，为了实现机械臂能够自动定位并寻找到最优路径，故提出了本申请的自动定位寻路方法。

步骤S200，根据所述当前位姿数据和所述位置数据计算最短路径；

确定了当前机械臂的位姿和手术位置后，即知道了起点和终点，基于空间之间两点之间直线最短，因此可以由这两点来获得最短路径的直线公式，即：

p(λ)＝(1-λ)*p₁+λ*p₂

式中p1为所述机械臂前端工具的空间坐标，p2为所述手术区域的空间坐标，λ为斜率参数。其中λ取值范围在0到1之间，具体取值和路径公式时的坐标系相关。

步骤S300，构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵为满秩，则求解得到所述机械臂的各个关节角度。

建立本申请实施例中机械臂和末端工具的角度公式，该公式为：

q＝J(q)^-1*V_e

式中q为所述机械臂的关节速度，V_e是所述机械臂的末端速度，j(q)^-1表示从关节速度向末端速度映射的雅克比矩阵的逆。

其中，结合步骤S200中确定的最短路径，当前所需要解决的问题就是，使得机械臂按照上述最短路径前进到达目标位置，以求雅克比矩阵的解来确定机械臂每个关节的关节角度。

由上述雅克比矩阵公式可知，当j(q)为0时，分母为0，则该式子没有意义，也就是说需要求j(q)是否存在满秩的情况，当求解后发现存在满秩的解，则根据解确定所述机械臂在到达手术位置时的各个关节角度，以此来确定最终的机械臂姿态，进而控制机械臂到达所述手术区域。

步骤S400，根据所述各个关节角度控制所述机械臂到达所述手术区域。

根据上述步骤，确定了最短路径以及选取了机械臂到手术区域后所应该呈现的姿态，则可以根据这两个条件来进行计算，确定手术接卸比各个关节在执行上述移动时所应该旋转的角度和方向，并且根据手术规范，限定机械臂移动的速度。

作为另一种可选的方案，当j没有满秩的情况，则意味着机械臂到达目标位置可能不存在唯一最优解，则还需要进一步获得一个较优的解，以用于尽量达到手术区域。

如图2所示，该方法还包括：

步骤500，根据所述手术区域的位置数据计算所述机械臂位于所述手术区域时的所有候选位姿数据，并将所有所述候选位姿数据和所述当前位姿数据进行差值计算。

首先会根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选的目标位姿的欧拉角坐标，由步骤S100可知，因为手术区位置确定，所以末端工具的位置也确定了，因此该候选位置可以为机械臂绕末端工具所在轴顺时针旋转360度后所产生的所有姿态数据，为了方便计算，可以每隔一度进行采样，最终采集360组可能的位姿数据。

具体而言，如图3所示，图中为机械臂俯视图，中间的圆形为机械臂末端工具200所在位置，周围的黑线为机械臂100进行顺时针或逆时针旋转后，通过计算模拟出来的位姿，每个模拟出来的机械臂100的位姿之间相隔1°，总共一圈会有360个机械臂位姿，而这些位姿显然只需要知道机械臂末端工具200的位置就可以直接通过计算得到，以欧拉角为例，若手术位置的坐标为：

(x₁，y₁，z₁，rx₁，ry₁，rz₁)。则所有候选位姿的欧拉角坐标为{x_n,y_n,z_n,rx_n,ry_n,rz_n}＝{x₁,y₁,z₁,rx₁,ry₁,rz₁}*{0,0,0,0,0,n}，其中n取值0到360的整数。得到360组与n有关的欧拉角。

步骤600，将计算得到最小差别值的所述候选位姿数据作为选取的位姿数据，根据所述选取的位姿数据和所述最短路径确定所述关节角度。

这些数据都是欧拉角数据，需要将这些欧拉角数据转换成四元数坐标，根据如下差值计算公式，来计算到和当前机械臂姿态最为接近的候选姿态。

Err＝|q_{1_1}-q_{n_1}|+|q_{1_2}-q_{n_2}|+|q_{1_3}-q_{n_3}|+|q_{1_4}-q_{n_4}|

式中，q_{1_1}至q_{1_4}为所述当前位姿数据的四元数数据，q_{n_1}至q_{n_4}为所述所有候选位姿数据的四元数数据，Err为所述差别值。

根据上述公式计算可以得到360个差别值，选差别值最小的那一组四元数对应的候选位姿为目标位姿，确定了目标位姿后，再将该四元数坐标转换回欧拉角坐标，以此来确定手术机械臂在手术位置的选取的位姿。因为四元数的变化是线性的，可以体现每个位姿之间的差别大小，而当差别值最小时，代表机械臂从当前姿态变动到对应的最终姿态所需要的步骤最少，变化最少，则在雅克比矩阵没有满秩解的情况，可以通过该方法遍历360组候选姿态来选取一个次优解。

本申请实施例的手术机械臂自动定位寻路方法，首先确定手术位置和当前机械臂的位置数据，确定最短路径，然后根据当前机械臂位姿和最短路径设立雅克比矩阵，通过求解雅克比矩阵的满秩的解来确定最优的机械臂目标姿态，若雅克比矩阵没有满秩的解，则通过遍历当机械臂末端工具在手术位置时，所有可能的机械臂位姿来和当前机械臂的位姿来进行差值计算，通过转换成四元数形式来进行差值计算，从而找到和当前位姿差别最小的目标位姿，以该目标位姿确定最终机械臂的关节角度，来控制机械臂运动。通过上述方法，使得机械臂可以有效的避免奇异位，在无法获得最优解的情况，也可以通过获取次优解的方式来决定机械臂的移动方式，保证了手术过程的安全和机械臂移动的精确。

实施例2

本申请实施例还提供一种手术机械臂自动定位寻路装置，如图4所示，包括：

定位模块10，用于获取机械臂的当前位姿数据以及手术区域的位置数据；

寻路模块20，用于根据所述当前位姿数据和所述位置数据计算最短路径；

计算模块30，用于构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵的解为满秩时，确定所述机械臂的各个关节角度。

控制模块40，用于根据关节角度控制所述机械臂到达所述手术区域。

位姿模拟模块50，用于若所述雅克比矩阵不为满秩，则根据所述手术区域的位置数据计算所述机械臂位于所述手术区域时的所有候选位姿数据，并将所有所述候选位姿数据和所述当前位姿数据进行差值计算；

差值计算模块60，用于将计算得到最小差值的候选位姿数据作为选取的位姿数据，根据所述选取的位姿数据和所述最短路径确定所述关节角度。

进一步的，本申请实施例还提供一种手术机器人，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行上述实施例中任一项所述的手术机械臂自动定位寻路方法。

进一步的，本申请实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行上述实施例中任一项所述的手术机械臂自动定位寻路方法。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种手术机械臂自动定位寻路方法，其特征在于，包括：

获取机械臂的当前位姿数据以及手术区域的位置数据；

根据所述当前位姿数据和所述位置数据计算最短路径；

构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵为满秩，则求解得到所述机械臂的各个关节角度；

根据所述各个关节角度控制所述机械臂到达所述手术区域。

2.根据权利要求1所述的手术机械臂自动定位寻路方法，其特征在于，还包括：

若所述雅克比矩阵不为满秩，则根据所述手术区域的位置数据计算所述机械臂位于所述手术区域时的所有候选位姿数据，并将所有所述候选位姿数据和所述当前位姿数据进行差值计算；

将计算得到最小差值的候选位姿数据作为选取的位姿数据，根据所述选取的位姿数据和所述最短路径确定所述关节角度。

3.根据权利要求2所述的手术机械臂自动定位寻路方法，其特征在于，所述机械臂包括前端工具，所述当前位姿数据包括所述前端工具的空间坐标以及机械臂围绕所述前端工具旋转的欧拉角；

将所有所述候选位姿数据和所述当前位姿数据进行差值计算包括：

根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选位姿的四元数坐标，将所述当前位姿的四元数坐标与所述所有候选的目标位姿四元数坐标进行所述差值计算；

根据所述选取的位姿数据和所述最短路径确定所述关节角度包括：

将所述选取的位姿的四元数坐标转换成相应的欧拉角坐标，根据所述选取的位姿的欧拉角坐标和所述最短路径确定所述关节角度。

4.根据权利要求3所述的手术机械臂自动定位寻路方法，其特征在于，所述根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选的目标位姿数据包括：

根据所述手术区域确定所述前端工具的目标位置，每隔一度，记录所述机械臂沿顺时针或逆时针围绕所述前端工具在所述目标位置时所形成的所有位姿数据，将所述形成的所有位姿数据作为所述候选的目标位姿数据。

5.根据权利要求2所述的手术机械臂自动定位寻路方法，其特征在于，所述差值计算的公式为：

Err＝|q_{1_1}-q_{n_1}|+|q_{1_2}-q_{n_2}|+|q_{1_3}-q_{n_3}|+|q_{1_4}-q_{n_4}|

式中，q_{1_1}至q_{1_4}为所述当前位姿数据的四元数数据，q_{n_1}至q_{n_4}为所述所有候选位姿数据的四元数数据，Err为所述差值。

6.根据权利要求1所述的手术机械臂自动定位寻路方法，其特征在于，所述雅克比矩阵公式为：

q＝J(q)^-1*V_e

式中q为所述机械臂的关节速度，V_e是所述机械臂的末端速度，j(q)^-1表示从关节速度向末端速度映射的雅克比矩阵式的逆。

7.根据权利要求1所述的手术机械臂自动定位寻路方法，其特征在于，所述最短路径为所述机械臂的前端工具的空间坐标到所述手术区域的空间坐标连成的直线；

所述最短路径的计算公式为：

p(λ)＝(1-λ)*p₁+λ*p₂

式中p1为所述机械臂的前端工具的空间坐标，p2为所述手术区域的空间坐标，λ为斜率参数。

8.一种手术机械臂自动定位寻路装置，其特征在于，包括：

定位模块，用于获取机械臂的当前位姿数据以及手术区域的位置数据；

寻路模块，用于根据所述当前位姿数据和所述位置数据计算最短路径；

计算模块，用于构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵为满秩，则求解得到所述机械臂的各个关节角度；

控制模块，根据所述各个关节角度控制所述机械臂到达所述手术区域用于。

9.一种手术机器人，其特征在于，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行权利要求1至7中任一项所述的手术机械臂自动定位寻路方法。

10.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上运行时执行权利要求1至7中任一项所述的手术机械臂自动定位寻路方法。

Images