[发明专利]一种腹腔镜手术中多机械臂的实时避障方法

说明书

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

技术领域

本发明涉及医疗机器人技术领域，特别涉及一种腹腔镜手术中多机械臂的实时避障方法。

背景技术

在腹腔镜外科手术中，医疗机器人应用广泛，大部分手术过程中，机器人和导航系统主要起辅助作用，让机器人自主完成决策，规划，执行等一系列操作是十分困难的；同时需要保证手术过程中病人的绝对安全，做到这一点，需要机器人系统能够提前判断危险点，并快速做出决策。

腹腔镜手术机器人通常配置多条机械臂，各个机械臂的工作空间存在重叠部分，不仅如此，实际手术场景中还可能存在各种工具、障碍物等等。如何避免各条机械臂互相碰撞以及机械臂与外界环境的碰撞，同时由机械臂自主规划一条无碰撞的可执行路径，一直是各大医疗手术机器人公司研究的热点。

上述技术方式的主要缺陷与不足在于：传统医疗手术机器人的自主性不足，不能提前判断可能的碰撞或危险，在检测到碰撞后不能自动调整机械臂执行路径。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种腹腔镜手术中多机械臂的实时避障方法。

为了实现上述目的，本发明的实施例提供一种腹腔镜手术中多机械臂的实时避障方法，包括如下步骤：

步骤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＝β_iw_i。

进一步，在所述步骤S3中，所述速度约束为：