一种腹腔镜手术中多机械臂的实时避障方法技术

本发明专利技术提出了一种腹腔镜手术中多机械臂的实时避障方法，包括：步骤S1，将腹腔镜手术机械臂的末端工具等效为胶囊体；步骤S2，对系统进行离散时域内的运动学建模；步骤S3，根据运动学建模，对所述末端工具分别设置速度约束和距离约束；步骤S4，定义代价函数；步骤S5，根据步骤S3中的速度约束和距离约束，以及步骤S4中的代价函数，建立优化方程；步骤S6，生成无碰撞的路径点。的路径点。的路径点。

【技术实现步骤摘要】
一种腹腔镜手术中多机械臂的实时避障方法


[0001]本专利技术涉及医疗机器人
，特别涉及一种腹腔镜手术中多机械臂的实时避障方法。

技术介绍

[0002]在腹腔镜外科手术中，医疗机器人应用广泛，大部分手术过程中，机器人和导航系统主要起辅助作用，让机器人自主完成决策，规划，执行等一系列操作是十分困难的；同时需要保证手术过程中病人的绝对安全，做到这一点，需要机器人系统能够提前判断危险点，并快速做出决策。
[0003]腹腔镜手术机器人通常配置多条机械臂，各个机械臂的工作空间存在重叠部分，不仅如此，实际手术场景中还可能存在各种工具、障碍物等等。如何避免各条机械臂互相碰撞以及机械臂与外界环境的碰撞，同时由机械臂自主规划一条无碰撞的可执行路径，一直是各大医疗手术机器人公司研究的热点。
[0004]上述技术方式的主要缺陷与不足在于：传统医疗手术机器人的自主性不足，不能提前判断可能的碰撞或危险，在检测到碰撞后不能自动调整机械臂执行路径。

技术实现思路

[0005]本专利技术的目的旨在至少解决所述技术缺陷之一。
[0006]为此，本专利技术的目的在于提出一种腹腔镜手术中多机械臂的实时避障方法。
[0007]为了实现上述目的，本专利技术的实施例提供一种腹腔镜手术中多机械臂的实时避障方法，包括如下步骤：
[0008]步骤S1，将腹腔镜手术机械臂的末端工具等效为胶囊体，其中，中间端是圆柱体，两端是半球体，两个胶囊体的距离定义为：
[0009][0010]其中，是为两个胶囊体轴线的距离，r
i
，r
j
分别为第i个和第j个胶囊的圆柱体半径；
[0011]步骤S2，对系统进行离散时域内的运动学建模如下：
[0012]x(k+1)＝x(k)+Bu(k)，
[0013]其中，表示两个工具的状态，为系数矩阵，Δt
c
为采样时间，是控制输入；
[0014]步骤S3，根据运动学建模，对所述末端工具分别设置速度约束和距离约束；
[0015]步骤S4，定义代价函数为：
[0016][0017]其中，p＝N/Δt
c
是预测时间步数，x
MPC
(k)是k时刻胶囊体期望状态，为k+i
时刻胶囊体期望预测状态；
[0018]步骤S5，根据步骤S3中的速度约束和距离约束，以及步骤S4中的代价函数，建立优化方程；
[0019]步骤S6，生成无碰撞的路径点，当一条机械臂运动时，其他机械臂视为障碍物处理，其中，设用R，G，O分别表示机械臂，目标点和障碍物在三维空间中的坐标位置；定义一个平面，机械臂能够经此平面无碰撞地达到目标点，用S
i
表示障碍物的集合，扫描障碍物范围，判断障碍物与平面是否有干涉；
[0020]找到移动的胶囊体和平面的交点T，Z
i
表示障碍物上离T最近的一个点，则无碰撞的中间路径点w
i
就是线段上的一个插值点；在每个时间步长内都计算一次路径点，得到路径点的集合：
[0021]W＝β
i
w
i
。
[0022]进一步，在所述步骤S3中，所述速度约束为：
[0023][0024]其中，是线性的最大速度约束，α(k)是置信水平系数。
[0025]进一步，在所述步骤S3中，所述距离约束为：
[0026][0027]其中，d
s
为用户定义的工具间最小距离。
[0028]进一步，在步骤S5中，所述优化方程为
[0029][0030][0031][0032][0033][0034]i＝0，...，p
‑1[0035]h＝0，...，N0[0036]j∈{r，j}
[0037]其中，和分别是胶囊体之间距离和胶囊体与障碍物之间的距离。
[0038]进一步，在步骤S5中，所述判断障碍物与平面是否有干涉，包括：
[0039]找到离R最近的障碍物上的两个点O
i，1，
O
i，2
，当这两个点满足以下两个条件时：
[0040](O
i，1
，O
i，2
)
·
n＝0
[0041]((O
i，1
+σ(O
i，2
‑
O
i
‑1))
‑
R1)
·
n＝0
[0042]则说明此障碍物与平面没有干涉，存在一条无碰撞的路径可以通过；如果没有干涉，则β＝0，如果有干涉，则β＜1。
[0043]进一步，在所述步骤S6中，设置平面法向量为：
[0044][0045]其中，R1，R2为此刻胶囊体轴线上的两个点，G1，G2为目标胶囊位置轴线上的两个点。
[0046]根据本专利技术实施例的腹腔镜手术中多机械臂的实时避障方法，、基于MPC方法的实时碰撞检测和避障策略，能够预测可能发生的碰撞，并根据障碍物的位置实时调整机械臂的运动轨迹。
[0047]本专利技术附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本专利技术的实践了解到。
附图说明
[0048]本专利技术的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0049]图1为根据本专利技术实施例的腹腔镜手术中多机械臂的实时避障方法的流程图；
[0050]图2a至图2c为根据本专利技术实施例的生成无碰撞路径点的示意图。
具体实施方式
[0051]下面详细描述本专利技术的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本专利技术，而不能理解为对本专利技术的限制。
[0052]本专利技术提出一种腹腔镜手术中多机械臂的实时避障方法，采用的是基于MPC方法的实时碰撞检测和避障策略。能够预测可能发生的碰撞，并根据障碍物的位置实时调整机械臂的运动轨迹。
[0053]如图1所示，本专利技术实施例的腹腔镜手术中多机械臂的实时避障方法，包括如下步骤：
[0054]步骤S1，将腹腔镜手术机械臂的末端工具等效为胶囊体。形状类似于胶囊，其中，中间端是圆柱体，两端是半球体，两个胶囊体的距离定义为：
[0055][0056]其中，是为两个胶囊体轴线的距离，r
i
，r
j
分别为第i个和第j个胶囊的圆柱体半径。
[0057]步骤S2，对系统进行离散时域内的运动学建模如下：
[0058]x(k+1)＝x(k)+Bu(k)，
[0059]其中，表示两个工具的状态，为系数矩阵，Δt
c
为采样时间，是控制输入。
[0060]步骤S3，根据运动学建模，对末端工具分别设置速度约束和距离约束。<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种腹腔镜手术中多机械臂的实时避障方法，其特征在于，包括如下步骤：步骤S1，将腹腔镜手术机械臂的末端工具等效为胶囊体，其中，中间端是圆柱体，两端是半球体，两个胶囊体的距离定义为：其中，是为两个胶囊体轴线的距离，r
i
，r
j
分别为第i个和第j个胶囊的圆柱体半径；步骤S2，对系统进行离散时域内的运动学建模如下：x(k+1)＝x(k)+Bu(k)，其中，表示两个工具的状态，为系数矩阵，Δt
c
为采样时间，是控制输入；步骤S3，根据运动学建模，对所述末端工具分别设置速度约束和距离约束；步骤S4，定义代价函数为：其中，p＝N/Δt
c
是预测时间步数，x
MPC
(k)是k时刻胶囊体期望状态，为k+i时刻胶囊体期望预测状态；步骤S5，根据步骤S3中的速度约束和距离约束，以及步骤S4中的代价函数，建立优化方程；步骤S6，生成无碰撞的路径点，当一条机械臂运动时，其他机械臂视为障碍物处理，其中，设用R,G,O分别表示机械臂，目标点和障碍物在三维空间中的坐标位置；定义一个平面，机械臂能够经此平面无碰撞地达到目标点，用S
i
表示障碍物的集合，扫描障碍物范围，判断障碍物与平面是否有干涉；找到移动的胶囊体和平面的交点T，Z
i
表示障碍物上离T最近的一个点，则无碰撞的中间路径点w
i
就是线段上的一个插值点；在每个时间步长内都计算一次路径点，得到路径点的集合：W＝β
i
w
i
。2.如权利要求1所述...

【专利技术属性】
技术研发人员：刘凯，韩峰涛，庹华，张航，于文进，韩建欢，张雷，马建涛，何刚，
申请(专利权)人：珞石北京科技有限公司，
类型：发明
国别省市：