CN114564009A - 一种手术机器人路径规划方法及系统 - Google Patents

Abstract

本发明为了解决手术机器人高自主水平手术中的安全性和准确性问题,公开了一种手术机器人路径规划方法及系统。在到达手术位姿阶段，利用改进人工势场算法对末端执行器的两个端点位置进行了约束，在不求取运动学逆解的情况下，能够准确驱动机械臂到达指定位姿，同时采用动态引力常数和分段斥力函数，解决路径规划中常见的局部极小值、障碍物附近目标不可达等问题；在执行规划路径阶段，利用原‑对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度方案，防止位置误差的积累。本发明能够让机器人在避障情况下平稳快速到达规划位姿，按照规划路径准确执行移动。

Description

一种手术机器人路径规划方法及系统

技术领域

本发明涉及手术导航技术领域，尤其涉及一种手术机器人路径规划方法和系统。

背景技术

近年来，手术导航系统以其高成功率和低创伤风险在临床手术中得到了广泛应用。作为手术导航的执行部分，手术机器人位姿规划的准确性和安全性可以极大地提高机器人辅助手术的整体效率。

目前，国际上对于手术机器人的研究主要分为两大方面，其一是主从式控制，代表产品有达芬奇机器人、Magellan机器人手术系统；另一方面是半自动控制，包括MAKO、Acrobot、ROSA系统。上述手术机器人虽然可以手动控制避免安全事故，比如术中与器械或病人的碰撞，但是需要医生的持续监督，并且学习操作机器人是一件耗时的工作。因此，对于更高自主水平的手术机器人路径规划方法的研究是必要的。

机器人辅助椎弓根螺钉植入手术是机器人手术中比较常见的手术类型。先前的研究已经证明，在不损害皮质骨完整性的情况下限制椎弓根螺钉的角度和轨迹，对于最大限度地提高脊柱疾患固定的准确性和安全性具有重要意义。对于自主性较高的椎弓根螺钉植入手术而言，路径规划算法应包含两部分：达到手术位姿以及执行规划路径。到达手术位姿是指机器人持螺钉到达螺钉植入的位置和方向，执行规划路径是指完成对准后沿规划路径植入。

然而，目前的路径规划方法多是针对目标位置或是关节空间进行的规划，没有考虑姿态要求，且对于人工势场算法中的局部极小值问题，添加虚拟目标点或者虚拟障碍物的方法并不适用于手术场景中位姿的对准。此外，在执行规划路径阶段，当前的研究没有权衡计算的复杂度、准确性和手术时的安全性。

发明内容

本发明的目的是通过以下技术方案实现的。

本发明为了解决上述背景提到的技术问题，公开了一种手术机器人路径规划方法及系统。在到达手术位姿阶段，利用改进人工势场算法对末端执行器的两个端点位置进行了约束，在不求取运动学逆解的情况下，能够准确驱动机械臂到达指定位姿，同时采用动态引力常数和分段斥力函数，解决路径规划中常见的局部极小值、障碍物附近目标不可达等问题；在执行规划路径阶段，利用原-对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度方案，防止了位置误差的积累。

具体的，根据本发明的一个方面，提供了一种手术机器人路径规划方法，包括以下步骤：

步骤1：建立机械臂与障碍物凸体模型，计算凸体之间的最近距离以及方向；

步骤2：根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿；

步骤3：在避免机械臂碰撞与关节极限的情况下，执行规划路径。

进一步地，步骤1所述建立机械臂模型方法是利用连杆等效圆柱面的上下底面建立机械臂的凸体模型；建立障碍物凸体模型的方法是将深度相机捕获的点云用快速凸包算法进行凸体化，以减小障碍物规则化后的体积；计算凸体之间最近距离的方法是GJK算法，对两凸体进行碰撞检测，并计算两者之间的最近距离以及对应的点。

进一步地，步骤2所述的根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿，包括：

通过对末端执行器的两个端点位置进行约束，在不求取运动学逆解的情况下，驱动机械臂到达指定位姿；

采用动态引力常数，计算末端执行器两端的雅可比矩阵以及引力矩；采用分段斥力函数计算最近距离点处的雅可比矩阵及斥力矩；

根据引力矩和斥力矩计算合力矩，并结合自适应运动步长和运动方向，确定机械臂下一步的位置。

进一步地，步骤3所述的执行规划路径，包括：利用原-对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度。

进一步地，所述对末端执行器的两个端点位置进行约束是通过在末端执行器两端构造引力实现的，方法如下：

其中，K_a为引力常数，p_goal1、p_goal2为末端执行器两端的目标位置，p₁(θ)、p₂(θ)为机械臂运动过程中末端执行器两端的实时位置，U_att1(θ)、U_att2(θ)为末端执行器两端的引力势能，F_att1(θ)、F_att2(θ)为末端执行器两端所受的引力。

进一步地，所述动态引力常数K_a表示如下：

K_a＝f(r,c)

其中，r为局部极小值出现的次数，c为预检测碰撞的次数，函数f表示引力常数随着r、c动态变化。

进一步地，所述分段斥力函数在每个连杆距离障碍物最近点处构造，其中末端执行器以外的连杆斥力与最近距离呈负相关关系，而末端执行器所受斥力与最近距离负相关，同时与末端执行器两端距离目标位姿之间的距离成正相关。

进一步地，所述自适应运动步长与实时误差相关，运动方向

由合力矩τ_total确定，表示为：

其中，τ_total由各连杆产生的斥力力矩以及末端执行器两端产生的引力力矩组合而成。

进一步地，所述利用原-对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，包括：计算多约束下的二次规划问题，再通过原-对偶神经网络进行求解，表示如下：minimizeω^Tω/2

J_oω≤v_o

ε^-≤ω≤ε⁺

其中，ω为关节速度，J(θ)代表机械臂各关节角为θ时的雅克比矩阵，

为末端执行器规划的线速度

和角速度v_a的组合，J_o为机械臂避障相关的雅克比矩阵，v_o为避障速度，ε^±代表关节速度的上下极限；

所述二次规划问题表示为如下的原-对偶神经网络：

其中，γ是一个正的设计参数，用于衡量神经网络的收敛率，I为单位矩阵，H和p为增广系数矩阵，P_Ω为投影算子，y为待优化量和对偶决策变量组成的矩阵，

为每一次迭代y的增量。

进一步地，所述基于每一步位置误差的实时规划速度，包括：利用末端执行器的实时位置与对应的规划位置之间的误差对规划的线速度

进行修正，如果实时位置比规划位置大，下一时刻的

就变小；如果实时位置比规划位置小，下一时刻的

就变大。

根据本发明的另一个方面，提供了一种手术机器人路径规划系统，包括：

定位模块，通过深度相机捕获障碍物点云，提供障碍物的实时位置；利用双目相机定位带有光学标记物的手术器械，确定手术位姿；

计算机处理模块，将目标位姿、障碍物信息变换到机械臂坐标下，计算机械臂的实时速度，实现机械臂安全准确地运动；

执行模块，接收计算机处理模块传递的机械臂实时速度的指令，控制手术器械执行规划路径。

本发明的有益效果在于：在到达手术位姿阶段，利用改进人工势场算法对末端执行器的两个端点位置进行了约束，在不求取运动学逆解的情况下，能够准确驱动机械臂到达指定位姿，同时采用动态引力常数和分段斥力函数，解决路径规划中常见的局部极小值、障碍物附近目标不可达等问题；在执行规划路径阶段，利用原-对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度方案，防止了位置误差的积累。为机器人手术提供了一种安全准确的手术路径规划方法。

附图说明

在附图中，除非另外规定，否则贯穿多个附图相同的附图标记表示相同或相似的部件或元素。这些附图不一定是按照比例绘制的。应该理解，这些附图仅描绘了根据本发明公开的一些实施方式，而不应将其视为是对本发明范围的限制。

图1是本发明具体实施方式的硬件设备结构图。

图2是本发明路径规划方法的流程图。

图3是本发明的机械臂连杆等效柱面图。

图4是本发明的穿刺针凸体化图。

图5是本发明的本发明的局部极小值实验配置图。

图6是本发明的本发明的障碍物附近目标不可达实验配置图。

图7是本发明的自适应步长与末端误差关系图。

图8是椎弓根螺钉植入示意图。

图9示出根据本发明实施例的手术机器人路径规划系统的构成图。

图10示出了本发明一实施例所提供的一种电子设备的结构示意图；

图11示出了本发明一实施例所提供的一种存储介质的示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

图1示出了根据本发明实施方式的硬件设施结构图；本发明提出一种手术机器人路径规划系统，包括双目相机1、深度相机2、工作站5、显示器6、手术器械7，机械臂9以及末端执行器10。深度相机2上有红外点阵投射器3和左右红外相机4，手术器械7上具有三个光学定位标记物8。双目相机1用于定位手术器械7的位姿，从而获取手术位姿；深度相机用于捕获障碍物点云信息，定位障碍物在空间中的分布；工作站5将手术器械，障碍物以及机械臂统一到同一坐标系后，进行算法的处理和指令的传递，将机械臂实时的运动过程在显示器6上展示；机械臂9接收到工作站5传递的指令后，持末端执行器10执行规划的路径。

如图2所示，本实施例公开了一种手术机器人路径规划方法，具体过程如下：

步骤1：建立机械臂与障碍物凸体模型，计算两者之间的最近距离以及方向。本实施例中采用的是德国库卡公司的LBR iiwa 7七自由度冗余机械臂作为研究对象，仿真实验在MATLAB 2020a平台上进行。将机械臂等效为以5条线段为轴线的等效圆柱，如图3所示，由正运动学计算第i个关节的坐标p_i(x_i,y_i,z_i)，设r_i为等效半径，则可求出连杆i的等效圆柱面，由于圆柱本身就是凸体，圆柱上下底面圆周上的点就可以代表整个圆柱在空间中的位置，以此简化计算。

对于障碍物的建模，本发明采用快速凸包算法尽可能少的增大障碍物的体积，保证工作空间的同时不会过多影响机械臂的实时控制，如图4所示。算法核心包括两方面：一方面是初始点的选取，另一方面是凸点的计算。

由深度相机给出障碍物的点云信息，从中选取3个初始点，规则为：选取z坐标最大的顶点p_a(x_a,y_a,z_a)，计算p_a与剩下所有顶点在水平面的夹角θ_x、θ_y，使θ_x、θ_y最小的两个点p_b(x_b,y_b,z_b)、p_c(x_c,y_c,z_c)与p_a共同组成平面f₁。

遍历剩余顶点，与初始顶点中的任意两点组成新的平面f₂，计算平面f₁与平面f₂的夹角θ_f，将使θ_f最大的面中的3个顶点重排序后作为新的初始点，以此类推，直到找出所有凸点。两平面的夹角可以表示为：

其中，

分别为平面f₁、f₂的法向量，通过初始点叉乘求得。至此完成了障碍物的建模。

得到机械臂与障碍物的凸体模型后，利用GJK算法计算两者之间的最近距离以及对应的点。算法的核心思想是将两个凸体的顶点依次相减得到闵科夫斯基差，如果两个物体相交，则闵科夫斯基差将包含原点，否则计算闵科夫斯基差形成的多面体与原点的最近距离以及最近距离点，此距离即为两个凸体之间的最近距离。需要注意的是，在每次迭代中，方向向量要朝着原点方向迭代，保证单纯形逐渐向原点靠近。

当单纯形迭代到距离原点最近时，根据单纯形中距离原点最近的点E计算出机械臂与障碍物上对应的最近距离点F、G。因此机械臂连杆i与障碍物k之间的最近距离为

方向为

步骤2：根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，平稳快速到达手术位姿。此阶段，任务有三，到达目标位姿、解决局部极小值和障碍物附近目标不可达问题。本实施例将针对以上三个任务做详细说明。

首先是到达目标位姿，通过在机械臂末端执行器两端构造引力势场函数，用两端点的位置来约束末端执行器的方向，避免了逆解的求取，方法如下：

其中，K_a为引力常数，p_goal1、p_goal2为末端执行器两端的目标位置，p₁(θ)、p₂(θ)为机械臂运动过程中末端执行器两端的实时位置，如图3所示。U_att1(θ)、U_att2(θ)为末端执行器两端的引力势能，F_att1(θ)、F_att2(θ)为末端执行器两端所受的引力。

其次是局部较小值问题，当机械臂运动的环境中存在两个及以上障碍物时，机械臂受到引力和多个斥力作用，此时很容易出现在某个位置合力为0的情况，机械臂就停在原地或是在局部极小值位置震荡，无法继续向目标运动。图5是本实施例出现局部极小值时的初始化环境配置图，为了逃离局部极小值，需要改变势场的分布，本发明采用动态引力常数来达到此目的，表示如下：

K_a＝f(r,c)

其中，r为局部极小值出现的次数；c为预检测碰撞的次数，即利用GJK算法计算机械臂下一时刻是否与障碍物发生碰撞；函数f表示引力常数K_a随着r、c动态变化。

最后是障碍物附近目标不可达问题，当障碍物离目标很近时，如图6所示，末端执行器在目标位姿处斥力很大，此时依据传统人工势场法构造势场函数可能会导致末端执行器在目标位姿处并非全局最小值，因此无法继续向目标运动。为了解决上述问题，本发明采用分段斥力函数，在每个连杆距离障碍物最近的点处构造处理函数，而在末端执行器上构造的函数引入末端位姿与目标位姿之间的误差，通过上述处理，末端执行器以外的连杆斥力与最近距离呈负相关关系，而末端执行器所受斥力与最近距离负相关，同时与末端执行器两端距离目标位姿之间的距离成正相关，从使机械臂各连杆能完成避障的同时，末端执行器也能到达障碍物附近的目标位姿。

以上获取了机械臂运动时的所需的虚拟力，本发明利用虚拟力产生的力矩来驱动机械臂运动，为防止合力过大导致机械臂运动幅度过大，本发明对力矩进行了归一化处理，利用归一化的力矩确定机械臂每一步运动的方向，幅度大小则通过自适应步长调整，使其在离目标位姿很远时运动幅度较大，反之则亦然。

运动方向

由合力矩τ_total确定，可以表示为：

其中，τ_total由各连杆产生的斥力力矩以及末端执行器两端产生的引力力矩组合而成。

自适应运动步长与实时误差相关，本实施例给定的函数为：

其中，a₁，a₂，step_max均为正常数，δ(θ)代表机械臂各关节角为θ时，末端执行器两端与目标之间的距离，图7给出了a₁不同值时，自适应步长与δ(θ)之间的关系，可以看出随着末端执行器里目标位姿逐渐接近，运动步长也逐渐减小。

步骤3：在避免机械臂碰撞与关节极限的情况下，准确柔顺地执行规划路径。本发明利用原-对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度方案，防止了位置误差的积累。

首先将末端执行器的姿态控制、避免机器人的碰撞和关节极限转化为多约束下的二次规划问题，表示如下：

minimizeω^Tω/2

J_oω≤v_o

ε^-≤ω≤ε⁺

其中，ω为关节速度，J(θ)代表机械臂各关节角为θ时的雅克比矩阵，

为末端执行器规划的线速度

和角速度v_a的组合，J_o为机械臂避障相关的雅克比矩阵，v_o为避障速度，ε^±代表关节速度的上下极限。

图8所示的规划路径即为本实施例的椎弓根螺钉植入路径，也就是r_d，由于末端执行器的实时位置是通过速度相对时间的积分计算得出，因此如果每一步的速度计算出现误差，末端执行器的实际路径与规划路径r_d之间的误差将会变得很大。因此，本发明根据每一步的实时误差来修正规划的线速度

如果实时位置比规划位置大，下一时刻的

就会变小；反之亦然。

上述二次规划问题可以用对偶理论来放松约束条件，再利用线性变分不等式将双边约束转化为单边约束，最后用分段线性投影方程转化为等式问题，通过递归的方式来对原始为题进行求解，也就是如下的原-对偶神经网络：

其中，γ是一个正的设计参数，用于衡量神经网络的收敛率，I为单位矩阵，H和p为增广系数矩阵，P_Ω为投影算子，y为待优化量和对偶决策变量组成的矩阵，

为每一次迭代y的增量。由于y中的前n个元素为ω，因此通过优化y便可以完成安全约束下的路径执行问题。

申请实施例提供了一种手术机器人路径规划系统，该系统用于执行上述实施例所述的手术机器人路径规划方法，如图9所示，该系统包括：

定位模块501，一方面通过深度相机捕获障碍物点云，提供障碍物的实时位置；另一方面，利用利用双目相机定位带有光学标记物的手术器械，从而确定手术位姿；

计算机处理模块502，将目标位姿，障碍物信息变换到机械臂坐标下，利用上述算法计算机械臂的实时速度，实现机械臂安全准确地运动；

执行模块503，接收计算机传递的机械臂实时速度的指令，持手术器械执行规划路径。

本发明的上述实施例提供的手术机器人路径规划系统与本发明实施例提供的手术机器人路径规划方法出于相同的发明构思，具有与其存储的应用程序所采用、运行或实现的方法相同的有益效果。

本发明实施方式还提供一种与前述实施方式所提供的手术机器人路径规划方法对应的电子设备，以执行上手术机器人路径规划方法。本发明实施例不做限定。

请参考图10，其示出了本发明的一些实施方式所提供的一种电子设备的示意图。如图10所示，所述电子设备2包括：处理器200，存储器201，总线202和通信接口203，所述处理器200、通信接口203和存储器201通过总线202连接；所述存储器201中存储有可在所述处理器200上运行的计算机程序，所述处理器200运行所述计算机程序时执行本发明前述任一实施方式所提供的手术机器人路径规划方法。

其中，存储器201可能包含高速随机存取存储器(RAM：Random Access Memory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。通过至少一个通信接口203(可以是有线或者无线)实现该系统网元与至少一个其他网元之间的通信连接，可以使用互联网、广域网、本地网、城域网等。

总线202可以是ISA总线、PCI总线或EISA总线等。所述总线可以分为地址总线、数据总线、控制总线等。其中，存储器201用于存储程序，所述处理器200在接收到执行指令后，执行所述程序，前述本发明实施例任一实施方式揭示的所述手术机器人路径规划方法可以应用于处理器200中，或者由处理器200实现。

处理器200可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器200中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器200可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器201，处理器200读取存储器201中的信息，结合其硬件完成上述方法的步骤。

本发明实施例提供的电子设备与本发明实施例提供的手术机器人路径规划方法出于相同的发明构思，具有与其采用、运行或实现的方法相同的有益效果。

本发明实施方式还提供一种与前述实施方式所提供的手术机器人路径规划方法对应的计算机可读存储介质，请参考图11，其示出的计算机可读存储介质为光盘30，其上存储有计算机程序(即程序产品)，所述计算机程序在被处理器运行时，会执行前述任意实施方式所提供的手术机器人路径规划方法。

需要说明的是，所述计算机可读存储介质的例子还可以包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他光学、磁性存储介质，在此不再一一赘述。

本发明的上述实施例提供的计算机可读存储介质与本发明实施例提供的手术机器人路径规划方法出于相同的发明构思，具有与其存储的应用程序所采用、运行或实现的方法相同的有益效果。

需要说明的是：

在此提供的算法和显示不与任何特定计算机、虚拟系统或者其它设备有固有相关。各种通用系统也可以与基于在此的示教一起使用。根据上面的描述，构造这类系统所要求的结构是显而易见的。此外，本发明也不针对任何特定编程语言。应当明白，可以利用各种编程语言实现在此描述的本发明的内容，并且上面对特定语言所做的描述是为了披露本发明的最佳实施方式。

在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

类似地，应当理解，为了精简本发明并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如下面的权利要求书所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。

本领域那些技术人员可以理解，可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。

此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在下面的权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。

本发明的各个部件实施例可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。本领域的技术人员应当理解，可以在实践中使用微处理器或者数字信号处理器(DSP)来实现根据本发明实施例的虚拟机的创建系统中的一些或者全部部件的一些或者全部功能。本发明还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者系统程序(例如，计算机程序和计算机程序产品)。这样的实现本发明的程序可以存储在计算机可读介质上，或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到，或者在载体信号上提供，或者以任何其他形式提供。

应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干系统的单元权利要求中，这些系统中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。

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

Claims (11)

1.一种手术机器人路径规划方法，其特征在于，该方法包含以下步骤：

步骤1：建立机械臂与障碍物凸体模型，计算凸体之间的最近距离以及方向；

步骤2：根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿；

步骤3：在避免机械臂碰撞与关节极限的情况下，执行规划路径。

2.根据权利要求1所述的方法，其特征在于，步骤1所述建立机械臂模型方法是利用连杆等效圆柱面的上下底面建立机械臂的凸体模型；建立障碍物凸体模型的方法是将深度相机捕获的点云用快速凸包算法进行凸体化，以减小障碍物规则化后的体积；计算凸体之间最近距离的方法是GJK算法，对两凸体进行碰撞检测，并计算两者之间的最近距离以及对应的点。

3.根据权利要求1或2所述的方法，其特征在于，步骤2所述的根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿，包括：

通过对末端执行器的两个端点位置进行约束，在不求取运动学逆解的情况下，驱动机械臂到达指定位姿；

采用动态引力常数，计算末端执行器两端的雅可比矩阵以及引力矩；采用分段斥力函数计算最近距离点处的雅可比矩阵及斥力矩；

根据引力矩和斥力矩计算合力矩，并结合自适应运动步长和运动方向，确定机械臂下一步的位置。

4.根据权利要求1或2所述的方法，其特征在于，步骤3所述的执行规划路径，包括：利用原-对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度。

5.根据权利要求3所述的方法，其特征在于，所述对末端执行器的两个端点位置进行约束是通过在末端执行器两端构造引力实现的，方法如下：

其中，K_a为引力常数，p_goal1、p_goal2为末端执行器两端的目标位置，p₁(θ)、p₂(θ)为机械臂运动过程中末端执行器两端的实时位置，U_att1(θ)、U_att2(θ)为末端执行器两端的引力势能，F_att1(θ)、F_att2(θ)为末端执行器两端所受的引力。

6.根据权利要求3所述的方法，其特征在于，所述动态引力常数K_a表示如下：

K_a＝f(r,c)

其中，r为局部极小值出现的次数，c为预检测碰撞的次数，函数f表示引力常数随着r、c动态变化。

7.根据权利要求3所述的方法，其特征在于，所述分段斥力函数在每个连杆距离障碍物最近点处构造，其中末端执行器以外的连杆斥力与最近距离呈负相关关系，而末端执行器所受斥力与最近距离负相关，同时与末端执行器两端距离目标位姿之间的距离成正相关。

8.根据权利要求3所述的方法，其特征在于，所述自适应运动步长与实时误差相关，运动方向

由合力矩τ_total确定，表示为：

其中，τ_total由各连杆产生的斥力力矩以及末端执行器两端产生的引力力矩组合而成。

9.根据权利要求4所述的方法，其特征在于，所述利用原-对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，包括：计算多约束下的二次规划问题，再通过原-对偶神经网络进行求解，表示如下：

minimizeω^Tω/2

subject to

J_oω≤v_o

ε^-≤ω≤ε⁺

其中，ω为关节速度，J(θ)代表机械臂各关节角为θ时的雅克比矩阵，

为末端执行器规划的线速度

和角速度v_a的组合，J_o为机械臂避障相关的雅克比矩阵，v_o为避障速度，ε^±代表关节速度的上下极限；

所述二次规划问题表示为如下的原-对偶神经网络：

其中，γ是一个正的设计参数，用于衡量神经网络的收敛率，I为单位矩阵，H和p为增广系数矩阵，P_Ω为投影算子，y为待优化量和对偶决策变量组成的矩阵，

为每一次迭代y的增量。

10.根据权利要求4所述的方法，其特征在于，所述基于每一步位置误差的实时规划速度，包括：利用末端执行器的实时位置与对应的规划位置之间的误差对规划的线速度

进行修正，如果实时位置比规划位置大，下一时刻的

就变小；如果实时位置比规划位置小，下一时刻的

就变大。

11.一种手术机器人路径规划系统，其特征在于，包括：

定位模块，通过深度相机捕获障碍物点云，提供障碍物的实时位置；利用双目相机定位带有光学标记物的手术器械，确定手术位姿；

计算机处理模块，将目标位姿、障碍物信息变换到机械臂坐标下，计算机械臂的实时速度，实现机械臂安全准确地运动；

执行模块，接收计算机处理模块传递的机械臂实时速度的指令，控制手术器械执行规划路径。

Images