CN115179297A - 手术机器人位置姿态控制联合避障关节极限的方法及系统 - Google Patents

Abstract

本发明公开了一种手术机器人位置姿态控制联合避障关节极限的方法及系统。所述方法包括：步骤1：建立机械臂与障碍物凸体模型，计算所述模型内凸体之间的最近距离以及方向；步骤2：根据目标位姿和障碍物的分布，实时规划末端执行器的速度；步骤3：结合末端规划速度与安全约束，控制手术机器人的运动。为了实现机器人安全准确的位置姿态控制，针对机器人自主手术中的三个阶段分别规划末端执行器的速度，将其表述为等式约束，同时将避障和关节限制表述为不等式约束，以最小化关节角速度为目标函数，形成二次规划问题，最后利用原‑对偶神经网络对二次规划问题进行求解，保证运动的实时性。

Description

手术机器人位置姿态控制联合避障关节极限的方法及系统

技术领域

本发明涉及手术导航技术领域，尤其涉及一种手术机器人位置姿态控制联合避障关节极限的方法和系统。

背景技术

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

外科手术机器人从用户和机器人系统之间关于工具运动的交互进行分类，主要有主从机器人、被动机器人、半主动机器人和主动机器人。在这四类机器人中，前三类机器人的研究较为广泛，对于这些机器人辅助系统，虽然可以通过手动控制来防止诸如外部物体对规划路径的干扰、机器人运动的限制等安全问题，但它们只在手术过程中提供一些帮助，手术过程需要医生的持续监督。同时，医生学习操作机器人执行手术是一项具有挑战性的任务。因此，面向主动手术机器人系统的研究变得尤为重要。

当前手术机器人主要用于骨科、神经外科、外科以及心血管领域，在这些手术中都需要控制机器人到达手术入口位置，再按照术前规划的手术路径操纵机器人执行手术。对于主动机器人而言，需要独立完成手术位姿对准和手术路径执行这两个过程。因此末端执行器的位姿控制是主动机器人系统中非常重要的一个部分。然而，目前机器人的路径规划方法多集中在关节空间或位置规划上，并不适用于手术过程中位姿对准场景。另外，对于位姿对准及路径执行两个不同的阶段的路径规划，分别对应与不确定路径与确定路径，没有很好的解决方案。同时，对于运动过程中安全性和准确性要求考虑不够全面。

发明内容

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

本发明为了解决上述背景提到的技术问题，公开了一种手术机器人位置姿态控制联合避障关节极限的方法及系统。在手术位置到达阶段，根据目标位置和障碍物的分布构造联合线速度，使末端执行器的路径远离障碍物向目标移动；在手术姿态对准阶段，根据末端执行器当前姿态与目标姿态之间的误差来调整末端执行器的角速度，完成姿态的对准；在手术路径执行阶段，利用每一步的误差对规划速度进行修正，避免了位置误差和姿态误差的累积；为了实现机器人安全准确的位置姿态控制，将上述规划的线速度和角速度表述为等式约束，避障和关节限制表述为不等式约束，最小化关节角速度为目标函数，形成二次规划问题，再利用原-对偶神经网络对二次规划问题进行求解，保证运动的实时性。

具体的，根据本发明的一个方面，提供了一种手术机器人位置姿态控制联合避障关节极限的方法，包括以下步骤：

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

步骤2：根据目标位姿和障碍物的分布，提出一种末端执行器实时规划速度策略；

步骤3：结合末端规划速度与安全约束，控制手术机器人安全准确地运动。

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

优选的，步骤2所述的实时规划速度策略分为三个阶段进行规划：机器人到达手术位置、机器人对准手术姿态以及机器人执行规划路径，然后将三个阶段的速度规划表述为联合速度规划策略，具体包括：

(1)在机器人到达手术位置阶段，利用人工势场法的思想，手术位置对机器人末端执行器产生吸引速度，障碍物对末端执行器产生排斥速度，结合吸引速度和排斥速度，规划一条末端执行器从初始位姿到达手术位置的无碰撞路径。

(2)在机器人对准手术姿态阶段，保持末端执行器位置不变的同时，根据末端执行器当前姿态与手术姿态之间的差距规划末端执行器的角速度。

(3)在机器人执行规划路径阶段，根据手术中对末端执行器位姿控制的需求，规划末端执行器的线速度和角速度，并利用末端执行器实际位姿与规划位姿之间的误差，修正下一步的线速度和角速度，使得机器人的运动更加精确。

(4)三个阶段末端执行器的联合速度规划策略所示如下：

其中，ω为关节速度，J_r代表械臂在不同阶段的雅克比矩阵，

为末端执行器在不同阶段的速度。

优选的，步骤3所述的控制机器人安全准确运动的方法是将安全约束表述为不等式约束，末端规划速度表述为等式约束，通过求解约束下的优化问题实现运动控制。

所述安全约束包含避免机器人与障碍物的碰撞和避免关节极限，末端规划速度为联合速度规划策略，优化的目标为最小化关节角速度，约束下的优化问题即为二次规划问题，表示如下：

minimizeω^Tω/2

subject to

J_oω≤v_o

ε^-≤ω≤ε⁺

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

优选地，求解二次规划问题采用的是原-对偶神经网络，表示如下：

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

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

根据本发明的另一个方面，提供了一种手术机器人位置姿态控制联合避障关节极限的系统，包括：

建模模块，用于建立机械臂与障碍物凸体模型，计算所述模型内凸体之间的最近距离以及方向；

速度规划模块，用于根据目标位姿和障碍物的分布，实时规划末端执行器的速度；

运动控制模块，用于结合末端规划速度与安全约束，控制手术机器人的运动。

本发明的有益效果在于：将手术中机器人的运动规划分为三个阶段进行控制，联合速度规划策略保证了末端执行器安全和准确，实时规划速度防止了位姿误差的累积，采用二次规划优化关节角速度使机械臂运动更加平稳，通过原-对偶神经网络求解实现了运动的实时控制。

附图说明

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

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

图2是本发明位置姿态控制联合避障关节极限的方法流程图。

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

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

图5是本发明末端执行器到达目标位置阶段速度规划示意图。

图6示出根据本发明实施例的手术机器人位置姿态控制联合避障关节极限的系统的构成图。

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

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

具体实施方式

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

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

图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之间的最近距离为

方向为

避障时只需计算障碍物k离机械臂的最近距离d_k，即

步骤2：根据目标位姿和障碍物的分布，提出一种末端执行器实时规划速度策略。此阶段的速度规划分为三个部分：到达手术位置、对准手术姿态以及执行规划路径。本实施例将针对以上三个任务做详细说明。

首先在机器人到达手术位置阶段，利用人工势场法的思想，使手术位置对机器人末端执行器产生吸引速度v_att，障碍物对末端执行器产生排斥速度v_rep，结合吸引速度和排斥速度，规划一条末端执行器从初始位姿到达手术位置的无碰撞路径，如图5所示。方法如下：

其中，

代表手术工具目标位置，

代表末端执行器当前位置，K_r为排斥速度的增益常数，

为障碍物k与机械臂之间最近距离的方向，由障碍物指向机械臂，d_s为排斥速度的作用范围。

结合吸引速度与排斥速度，末端执行器的线速度v_r可以表示为：

其中，v_r为正常数，表示末端执行器每一步运动速度的大小。

此阶段末端执行器的速度

即为线速度，表示为：

然后在机器人对准手术姿态阶段，保持末端执行器位置不变的同时，根据末端执行器当前姿态与手术姿态之间的差距规划末端执行器的角速度。本实施例利用手术工具的目标位置与末端执行器的实时位置做补偿，防止机器人在姿态对准过程产生位置误差，因此末端执行器线速度v_a可用公式表示为：

v_a＝p_g-p_c

本实施例中工具坐标系旋转矩阵R_c的z轴分量即为当前末端执行器的姿态o_c，可用公式表示为：

o_c＝R_c*n_z

其中，n_z＝[001]^T。本实施例根据末端执行器的目标姿态o_d与当前姿态o_c计算两者之间的转轴

用公式表示为：

则o_c以

为转轴旋转到o_d的角度θ_a可用公式表示为：

本实施中，末端执行器以恒定的速度大小完成姿态的对准，则每一步运动的角速度大小ω_a可以表示为：

其中，T_a表示完成姿态对准的时间。因此末端执行器的角速度ω_a可以表示为：

因此末端在姿态对准阶段的速度可以表示为：

最后在机器人执行器手术路径阶段，根据手术中对末端执行器位姿控制的需求，规划末端执行器的线速度和角速度，并利用末端执行器实际位姿与规划位姿之间的误差，修正下一步的线速度和角速度。

本实施例假设已知术前规划的末端执行器的位置p_d和姿态o_d，分别表示为：

其中，

表示末端执行器t时刻规划的位置，

表示末端执行器t时刻规划的姿态。则t时刻对应的线速度

可以表示为：

本实施例中角速度通过旋转角度θ_d与转轴

共同确定，因此t时刻对应的角速度

表示如下：

其中，

表示t时刻末端执行器围绕转轴

旋转的角度，

与

的计算如下所示：

将末端执行器规划的线速度与角速度写成集合的形式，表示如下：

由于末端执行器的位姿是由末端速度对时间的积分计算出的结果，因此每一时刻计算出的速度都会影响末端执行器最终的路径。本实施例将末端执行器上一时刻的位姿误差引入到当前时刻规划的速度中，如果上一时刻末端执行器所在的位姿没到达规划的位姿，则当前时刻规划的速度就大于原来规划的速度，弥补上一时刻没到达指定位姿而引起误差的缺陷。

修正后t时刻末端执行器的线速度

可以表示为：

其中，k_p为正常数，

代表末端执行器在t时刻实时的位置。因此，末端执行器实时的线速度v_c可以表示为：

本实施例对t时刻末端执行器的角速度也进行同样的修正，表示如下：

其中，

为t时刻末端执行器绕转轴

旋转的实际角度，

与

的计算如下所示：

其中，

为t时刻末端执行器的实时姿态，本实施例中此姿态为工具坐标系在t时刻相对基坐标的旋转矩阵

的z轴分量方向。

则实时的角速度速度ω_c可以表示为：

因此末端在姿态对准阶段的速度可以表示为：

以上实现了三个不同阶段末端执行器速度的规划，本发明将三个阶段末端执行器的速度进行统一表示，形成联合速度规划策略，所示如下：

其中，ω为关节速度，J_r代表械臂在不同阶段的雅克比矩阵，

为末端执行器在不同阶段的速度。本实施例中，J_r与

表示如下：

其中J_p为末端执行器位置雅克比矩阵，J为末端执行器雅克比矩阵。

步骤3：结合规划速度与安全约束，控制手术机器人安全准确地运动。本发明中控制机器人安全准确运动的方法是将安全约束表述为不等式约束，末端规划速度表述为等式约束，通过求解约束下的优化问题实现运动控制。

具体做法为：将避免机器人与障碍物的碰撞和避免关节极限表述为安全约束，通过联合速度规划实现位姿控制，以最小化关节角速度为优化的目标。则约束下的优化问题即为二次规划问题，表示如下：

minimizeω^Tω/2

subject to

J_oω≤v_o

ε^-≤ω≤ε⁺

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

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

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

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

申请实施例提供了一种手术机器人位置姿态控制联合避障关节极限的系统，该系统用于执行上述实施例所述的手术机器人位置姿态控制联合避障关节极限的方法，如图6所示，该系统包括：

建模模块501，用于建立机械臂与障碍物凸体模型，计算所述模型内凸体之间的最近距离以及方向；

速度规划模块502，用于根据目标位姿和障碍物的分布，实时规划末端执行器的速度；

运动控制模块503，用于结合末端规划速度与安全约束，控制手术机器人的运动。

本发明的上述实施例提供的手术机器人位置姿态控制联合避障关节极限的系统与本发明实施例提供的手术机器人位置姿态控制联合避障关节极限的方法出于相同的发明构思，具有与其存储的应用程序所采用、运行或实现的方法相同的有益效果。

本发明实施方式还提供一种与前述实施方式所提供的手术机器人位置姿态控制联合避障关节极限的方法对应的电子设备，以执行上手术机器人位置姿态控制联合避障关节极限的方法。本发明实施例不做限定。

请参考图7，其示出了本发明的一些实施方式所提供的一种电子设备的示意图。如图7所示，所述电子设备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中的信息，结合其硬件完成上述方法的步骤。

本发明实施例提供的电子设备与本发明实施例提供的手术机器人位置姿态控制联合避障关节极限的方法出于相同的发明构思，具有与其采用、运行或实现的方法相同的有益效果。

本发明实施方式还提供一种与前述实施方式所提供的手术机器人位置姿态控制联合避障关节极限的方法对应的计算机可读存储介质，请参考图8，其示出的计算机可读存储介质为光盘30，其上存储有计算机程序(即程序产品)，所述计算机程序在被处理器运行时，会执行前述任意实施方式所提供的手术机器人位置姿态控制联合避障关节极限的方法。

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

本发明的上述实施例提供的计算机可读存储介质与本发明实施例提供的手术机器人位置姿态控制联合避障关节极限的方法出于相同的发明构思，具有与其存储的应用程序所采用、运行或实现的方法相同的有益效果。

需要说明的是：

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

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

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

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

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

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

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

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

Claims (11)

1.一种手术机器人位置姿态控制联合避障关节极限的方法，其特征在于，该方法包含以下步骤：

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

步骤2：根据目标位姿和障碍物的分布，实时规划末端执行器的速度；

步骤3：结合末端规划速度与安全约束，控制手术机器人的运动。

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

3.根据权利要求1所述的方法，其特征在于，步骤2所述的实时规划末端执行器的速度包括三个阶段：机器人到达手术位置、机器人对准手术姿态以及机器人执行规划路径。

4.根据权利要求1所述的方法，其特征在于，步骤3所述的控制手术机器人运动的方法是将安全约束表述为不等式约束，末端规划速度表述为等式约束，通过求解约束下的优化问题实现运动控制。

5.根据权利要求3所述的方法，其特征在于，在机器人到达手术位置阶段，利用人工势场法，手术位置对机器人末端执行器产生吸引速度，障碍物对末端执行器产生排斥速度，结合吸引速度和排斥速度，规划一条末端执行器从初始位姿到达手术位置的无碰撞路径。

6.根据权利要求3所述的方法，其特征在于，在机器人对准手术姿态阶段，保持末端执行器位置不变的同时，根据末端执行器当前姿态与手术姿态之间的差距规划末端执行器的角速度。

7.根据权利要求3所述的方法，其特征在于，在机器人执行规划路径阶段，根据手术中对末端执行器位姿控制的需求，规划末端执行器的线速度和角速度，并利用末端执行器实际位姿与规划位姿之间的误差，修正下一步的线速度和角速度。

8.根据权利要求3所述的方法，其特征在于，三个阶段末端执行器的联合速度规划策略所示如下：

其中，ω为关节速度，J_r代表械臂在不同阶段的雅克比矩阵，

为末端执行器在不同阶段的速度。

9.根据权利要求4所述的方法，其特征在于，安全约束包含避免机器人与障碍物的碰撞和避免关节极限，末端规划速度为联合速度规划策略，优化的目标为最小化关节角速度，约束下的优化问题即为二次规划问题，表示如下：

minimize ω^Tω/2

subject to

J_oω≤v_o

ε^-≤ω≤ε⁺

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

10.根据权利要求9所述的方法，其特征在于，求解二次规划问题采用的是原-对偶神经网络，表示如下：

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

为每一次迭代y的增量。

11.一种手术机器人位置姿态控制联合避障关节极限的系统，其特征在于，包括：

建模模块，用于建立机械臂与障碍物凸体模型，计算所述模型内凸体之间的最近距离以及方向；

速度规划模块，用于根据目标位姿和障碍物的分布，实时规划末端执行器的速度；

运动控制模块，用于结合末端规划速度与安全约束，控制手术机器人的运动。

Images