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

权利要求书

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

2.如权利要求1所述的腹腔镜手术中多机械臂的实时避障方法，其特征在于，在所述步骤S3中，所述速度约束为：

其中，是线性的最大速度约束，α(k)是置信水平系数。

3.如权利要求1所述的腹腔镜手术中多机械臂的实时避障方法，其特征在于，在所述步骤S3中，所述距离约束为：

其中，d_s为用户定义的工具间最小距离。

4.如权利要求1所述的腹腔镜手术中多机械臂的实时避障方法，其特征在于，在步骤S5中，所述优化方程为

i＝0，...，p-1

h＝0，...，N₀

j∈{r，j}

其中，和分别是胶囊体之间距离和胶囊体与障碍物之间的距离。

5.如权利要求1所述的腹腔镜手术中多机械臂的实时避障方法，其特征在于，在步骤S5中，所述判断障碍物与平面是否有干涉，包括：

找到离R最近的障碍物上的两个点O_i，1，O_i，2，当这两个点满足以下两个条件时：

(O_i，1，O_i，2)·n＝0

((O_i，1+σ(O_i，2-O_i-1))-R₁)·n＝0

则说明此障碍物与平面没有干涉，存在一条无碰撞的路径可以通过；如果没有干涉，则β＝0，如果有干涉，则β＜1。

6.如权利要求1所述的腹腔镜手术中多机械臂的实时避障方法，其特征在于，在所述步骤S6中，设置平面法向量为：

其中，R1,R2为此刻胶囊体轴线上的两个点，G1,G2为目标胶囊位置轴线上的两个点。