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

本发明专利技术为了解决手术机器人高自主水平手术中的安全性和准确性问题,公开了一种手术机器人路径规划方法及系统。在到达手术位姿阶段，利用改进人工势场算法对末端执行器的两个端点位置进行了约束，在不求取运动学逆解的情况下，能够准确驱动机械臂到达指定位姿，同时采用动态引力常数和分段斥力函数，解决路径规划中常见的局部极小值、障碍物附近目标不可达等问题；在执行规划路径阶段，利用原

【技术实现步骤摘要】
一种手术机器人路径规划方法及系统


[0001]本专利技术涉及手术导航
，尤其涉及一种手术机器人路径规划方法和系统。

技术介绍

[0002]近年来，手术导航系统以其高成功率和低创伤风险在临床手术中得到了广泛应用。作为手术导航的执行部分，手术机器人位姿规划的准确性和安全性可以极大地提高机器人辅助手术的整体效率。
[0003]目前，国际上对于手术机器人的研究主要分为两大方面，其一是主从式控制，代表产品有达芬奇机器人、Magellan机器人手术系统；另一方面是半自动控制，包括MAKO、Acrobot、ROSA系统。上述手术机器人虽然可以手动控制避免安全事故，比如术中与器械或病人的碰撞，但是需要医生的持续监督，并且学习操作机器人是一件耗时的工作。因此，对于更高自主水平的手术机器人路径规划方法的研究是必要的。
[0004]机器人辅助椎弓根螺钉植入手术是机器人手术中比较常见的手术类型。先前的研究已经证明，在不损害皮质骨完整性的情况下限制椎弓根螺钉的角度和轨迹，对于最大限度地提高脊柱疾患固定的准确性和安全性具有重要意义。对于自主性较高的椎弓根螺钉植入手术而言，路径规划算法应包含两部分：达到手术位姿以及执行规划路径。到达手术位姿是指机器人持螺钉到达螺钉植入的位置和方向，执行规划路径是指完成对准后沿规划路径植入。
[0005]然而，目前的路径规划方法多是针对目标位置或是关节空间进行的规划，没有考虑姿态要求，且对于人工势场算法中的局部极小值问题，添加虚拟目标点或者虚拟障碍物的方法并不适用于手术场景中位姿的对准。此外，在执行规划路径阶段，当前的研究没有权衡计算的复杂度、准确性和手术时的安全性。

技术实现思路

[0006]本专利技术的目的是通过以下技术方案实现的。
[0007]本专利技术为了解决上述背景提到的技术问题，公开了一种手术机器人路径规划方法及系统。在到达手术位姿阶段，利用改进人工势场算法对末端执行器的两个端点位置进行了约束，在不求取运动学逆解的情况下，能够准确驱动机械臂到达指定位姿，同时采用动态引力常数和分段斥力函数，解决路径规划中常见的局部极小值、障碍物附近目标不可达等问题；在执行规划路径阶段，利用原
‑
对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度方案，防止了位置误差的积累。
[0008]具体的，根据本专利技术的一个方面，提供了一种手术机器人路径规划方法，包括以下步骤：
[0009]步骤1：建立机械臂与障碍物凸体模型，计算凸体之间的最近距离以及方向；
[0010]步骤2：根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿；
[0011]步骤3：在避免机械臂碰撞与关节极限的情况下，执行规划路径。
[0012]进一步地，步骤1所述建立机械臂模型方法是利用连杆等效圆柱面的上下底面建立机械臂的凸体模型；建立障碍物凸体模型的方法是将深度相机捕获的点云用快速凸包算法进行凸体化，以减小障碍物规则化后的体积；计算凸体之间最近距离的方法是GJK算法，对两凸体进行碰撞检测，并计算两者之间的最近距离以及对应的点。
[0013]进一步地，步骤2所述的根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿，包括：
[0014]通过对末端执行器的两个端点位置进行约束，在不求取运动学逆解的情况下，驱动机械臂到达指定位姿；
[0015]采用动态引力常数，计算末端执行器两端的雅可比矩阵以及引力矩；采用分段斥力函数计算最近距离点处的雅可比矩阵及斥力矩；
[0016]根据引力矩和斥力矩计算合力矩，并结合自适应运动步长和运动方向，确定机械臂下一步的位置。
[0017]进一步地，步骤3所述的执行规划路径，包括：利用原
‑
对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度。
[0018]进一步地，所述对末端执行器的两个端点位置进行约束是通过在末端执行器两端构造引力实现的，方法如下：
[0019][0020][0021]其中，K
a
为引力常数，p
goal1
、p
goal2
为末端执行器两端的目标位置，p1(θ)、p2(θ)为机械臂运动过程中末端执行器两端的实时位置，U
att1
(θ)、U
att2
(θ)为末端执行器两端的引力势能，F
att1
(θ)、F
att2
(θ)为末端执行器两端所受的引力。
[0022]进一步地，所述动态引力常数K
a
表示如下：
[0023]K
a
＝f(r,c)
[0024]其中，r为局部极小值出现的次数，c为预检测碰撞的次数，函数f表示引力常数随着r、c动态变化。
[0025]进一步地，所述分段斥力函数在每个连杆距离障碍物最近点处构造，其中末端执行器以外的连杆斥力与最近距离呈负相关关系，而末端执行器所受斥力与最近距离负相关，同时与末端执行器两端距离目标位姿之间的距离成正相关。
[0026]进一步地，所述自适应运动步长与实时误差相关，运动方向由合力矩τ
total
确定，表示为：
[0027][0028]其中，τ
total
由各连杆产生的斥力力矩以及末端执行器两端产生的引力力矩组合而成。
[0029]进一步地，所述利用原
‑
对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，包括：计算多约束下的二次规划问题，再通过原
‑
对偶神经网络进行求解，表示如下：minimizeω
T
ω/2
[0030][0031]J
o
ω≤v
o
[0032]ε
‑
≤ω≤ε
+
[0033]其中，ω为关节速度，J(θ)代表机械臂各关节角为θ时的雅克比矩阵，为末端执行器规划的线速度和角速度v
a
的组合，J
o
为机械臂避障相关的雅克比矩阵，v
o
为避障速度，ε
±
代表关节速度的上下极限；
[0034]所述二次规划问题表示为如下的原
‑
对偶神经网络：
[0035][0036]其中，γ是一个正的设计参数，用于衡量神经网络的收敛率，I为单位矩阵，H和p为增广系数矩阵，P
Ω
为投影算子，y为待优化量和对偶决策变量组成的矩阵，为每一次迭代y的增量。
[0037]进一步地，所述基于每一步位置误差的实时规划速度，包括：利用末端执行器的实时位置与对应的规划位置之间的误差对规划的线速度进行修正，如果实时位置比规划位置大，下一时刻的就变小；如果实时位置比规划位置小，下一时刻的就变大。
[0038]根据本专利技术的另一个方面，提供了一种手术本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种手术机器人路径规划方法，其特征在于，该方法包含以下步骤：步骤1：建立机械臂与障碍物凸体模型，计算凸体之间的最近距离以及方向；步骤2：根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿；步骤3：在避免机械臂碰撞与关节极限的情况下，执行规划路径。2.根据权利要求1所述的方法，其特征在于，步骤1所述建立机械臂模型方法是利用连杆等效圆柱面的上下底面建立机械臂的凸体模型；建立障碍物凸体模型的方法是将深度相机捕获的点云用快速凸包算法进行凸体化，以减小障碍物规则化后的体积；计算凸体之间最近距离的方法是GJK算法，对两凸体进行碰撞检测，并计算两者之间的最近距离以及对应的点。3.根据权利要求1或2所述的方法，其特征在于，步骤2所述的根据目标位姿和障碍物的分布，驱动机械臂避开障碍物，到达手术位姿，包括：通过对末端执行器的两个端点位置进行约束，在不求取运动学逆解的情况下，驱动机械臂到达指定位姿；采用动态引力常数，计算末端执行器两端的雅可比矩阵以及引力矩；采用分段斥力函数计算最近距离点处的雅可比矩阵及斥力矩；根据引力矩和斥力矩计算合力矩，并结合自适应运动步长和运动方向，确定机械臂下一步的位置。4.根据权利要求1或2所述的方法，其特征在于，步骤3所述的执行规划路径，包括：利用原
‑
对偶神经网络控制末端执行器的姿态、避免机器人的碰撞和关节极限，基于每一步位置误差的实时规划速度。5.根据权利要求3所述的方法，其特征在于，所述对末端执行器的两个端点位置进行约束是通过在末端执行器两端构造引力实现的，方法如下：束是通过在末端执行器两端构造引力实现的，方法如下：其中，K
a
为引力常数，p
goal1
、p
goal2
为末端执行器两端的目标位置，p1(θ)、p2(θ)为机械臂运动过程中末端执行器两端的实时位置，U
att1
(θ)、U
att2
(θ)为末端执行器两端的引力势能，F
att1
(θ)、F
att2
(θ)为末端执行器两端所受的引力。6.根据权利要求3所述的方法，其特征在于，所述动态引力常数K
a
表示如下：K
a
＝f(r,c)其中，r为局部极小值出现的次数，c为预检测碰撞的次数，函数f表示引力常数随着r、c动态变化。7.根据权利要求3所述的方法，...

【专利技术属性】
技术研发人员：张楠，武博，郝林佳，王宇，刘冬冬，
申请(专利权)人：首都医科大学，
类型：发明
国别省市：