面向穿刺针的蜂群觅食策略粒子群优化的路径规划方法

本发明属于机器人运动规划，面向穿刺针的蜂群觅食策略粒子群优化的路径规划方法。

背景技术：

1、经皮介入诊疗是现代临床医学微创外科的重要组成部分，也是病理检查、局部麻醉、局部给药的重要手段。在实际临床穿刺操作过程中，医生往往需要使用ct或超声来观察穿刺针的位置，这使得医生直接暴露在辐射环境中，会对医生的健康造成一定的损害。另外，如果针尖在穿刺过程中遇到障碍物，医生只能通过调整针尾来改变针尖的位置。这种手术增加了对患者额外伤害的风险，也增加了患者在手术过程中的痛苦，延长了患者术后恢复的时间。

2、穿刺手术的难点在于：在有效避开路径中的神经、动脉、血管、骨骼等障碍物的前提下，准确到达病灶。近年来，针对医疗机器人的发展提出了各种控制和规划方法，但这些研究未能为复杂的穿刺问题提供有效的控制和路径规划方法。因此，如何设计机器人穿刺作业的自动路径规划方法就显得尤为重要。

3、现有的斜尖柔性穿刺针路径规划方法虽然在单目标靶点单障碍和单目标靶点多障碍环境下有较好的规划精度，但是对于在较为复杂环境情况下进行路径规划的能力并不强。另一方面，对于多目标靶点的路径规划问题，传统进针逻辑使得总的穿刺路径长度较长，导致穿刺过程中破坏的组织较多。针对上述问题，有必要对智能运动规划算法在穿刺手术中的应用进行研究，以提高复杂环境中的穿刺效率和精度；

技术实现思路

1、针对现有技术中存在的上述问题，本发明的目的在于提供一种基于智能粒子群的路径规划算法，应用于穿刺机器人手术作业中，解决复杂环境适下应性差、穿刺规划精度低等问题。

2、本发明提供如下技术方案：面向穿刺针的蜂群觅食策略粒子群优化的路径规划方法，包括以下步骤：

3、1)假设柔性针在轴向上为刚性连接，即在穿刺过程中穿刺针不会发生轴向上的形变；

4、2)以针尖前进方向为z轴方向，建立穿刺针运动学模型得到参数向量具体公式如下：

5、

6、其中，分别为穿刺针在坐标系下三个方向的速度，α,β,γ分别为针尖在世界坐标系下围绕x,y,z轴的旋转角度，k为穿刺针在穿刺过程中的曲线斜率，v为针尖的前进速度，ω为针轴向旋转的角速度；

7、对模型进一步简化：将控制变量分别简化为与时间无关的物理量，即v,ω分别简化为进针距离s和针尖旋转角度θ，则运动模型参数向量进一步表示为：

8、

9、模型的控制采用停转策略，即插针动作和旋转针尖动作分开进行，同时只能执行插针或旋转针尖动作中的一个；

10、3)通过控制序列θ＝((s1,θ1),(s2,θ2),...,(sn,θn))唯一确定穿刺针的路径，其中n为路径的段数；设定包含路径长度、偏差与碰撞惩罚函数的目标优化函数，其具体的表达式如下：

11、y＝k1ys(θ)+yd(θ)+k2ytip(θ)

12、ytip(θ)＝|ptip(θ)-pd|

13、

14、

15、

16、其中，ys为路径长度函数，si为路径第i段的长度；ytip为目标点与针尖位置之间的误差函数；ptip(θ),pd分别为穿刺针实际位置与期望位置；yd为判断是否与障碍物碰撞的函数，j表示第j个障碍物，假设共有m个，y(d)为单个障碍物的碰撞函数；d,d分别为障碍物与针尖之间的实际距离与安全距离；k1,k2分别为路径长度函数和路径误差函数的权值；

17、4)将步骤3)中的目标优化函数作为蜂群觅食策略粒子群优化算法的适应度值，进行参数调整，得到更优的穿刺参考路径；

18、5)采用回退进针的策略来减小多靶点路径的整体穿刺路径长度，完成对所有目标靶点的穿刺路径规划。

19、进一步的，所述步骤2)中，穿刺针的运动模型的所有变量ξg＝[x,y,z,α,β,γ]t是基于世界坐标系，而对于针尖速度和旋转角速度则是基于针体坐标，具体由以下的公式进行推导：

20、

21、vp＝vv1+ωv2

22、其中，v1＝[0 0 1 k 0 0]t,v2＝[0 0 0 0 0 1]t；根据针体坐标与世界坐标的速度变换方程，可得vp＝qvg，且其中ptgp是世界坐标和针体坐标的旋转矩阵，使用时，当针和电磁定位系统的摆放位置确定下来后进行标定后唯一确定；pω为刚体定点运动的欧拉运动学系数矩阵，该矩阵建立了针体坐标ψp和世界坐标系ψg之间的角速度变换关系，

23、进一步的，所述步骤3)中，将整条穿刺路径分为多段标准圆弧且由控制序列θ＝((s1,θ1),(s2,θ2),...,(sn,θn))唯一确定，进一步假设：斜尖柔性穿刺针针尖路径的曲率是恒定不变的；针尖将以固定半径做圆弧运动，且运动方向由针尖朝向所决定；则三维空间下的路径转换过程如下：

24、3.1)在二维空间中，每段圆弧路径由第i段路径圆弧的起点坐标qi，qi处路径圆弧的切向量ni，以及所在弧线段所在圆心oi和弧长si唯一确定；在已知每一段路径圆弧的半径均为r的前提下，得到第i段圆弧的弧度为：

25、

26、于是，qi，ni，oi和si之间的转换公式为：

27、qi+1＝qicosσi+(1-cosσi)oi+rqisinσi

28、oi+1＝2qi+1-oi

29、ni+1＝(1-cosσi)oi+1+qi+1cosσi-qi

30、3.2)在三维空间中，各个圆弧路径段的圆心坐标之间的变换关系为：

31、oi+1＝λ2+qi+1

32、λ2＝λ1cosθi+1+ni+1×λ1sinθi+1+ni+1(ni+1*λ1)(1-cosθi+1)

33、其中，λ1,λ2为辅助向量，分别为，λ1＝oi-qi+1,λ2＝oi+1-qi+1，θi为三维空间中针尖的旋转角度。

34、进一步的，所述步骤4)中优化算法的具体过程如下：

35、4.1)初始化算法参数；

36、4.2)对各粒子的速度和位置进行更新，并根据各粒子的更新情况对其计数器count进行调整；

37、4.3)根据各粒子的适应度值计算各粒子被选中的概率n(xi)，并使用轮盘赌选择策略挑选粒子，具体的计算公式为：

38、

39、

40、其中，f(pbesti)为第i个粒子的最优适应度值，pi为粒子使用轮盘赌之后概率结果，n为粒子群中的粒子数量；

41、4.4)对被选中的粒子，对该粒子的速度和位置进行更新，并根据该粒子的更新情况对其计数器count进行调整；

42、4.5)遍历各粒子的计数器count，如果其中一个粒子的计数器值达到设定上限，则将该粒子重新初始化；

43、4.6)判断算法是否达到迭代停止条件，如果不满足，则返回步骤4.2，如果满足，则停止计算，输出结果。

44、进一步的，所述4.2)和4.4)中，对于粒子群的位置、速度更新和计数器count调整，具体的步骤如下：

45、4.2.1)速度、位置更新公式表达为：

46、vi(t+1)＝τvi(t)+c1r1(pbesti-xi(t))+c2r2(gbest-xi(t))

47、xi(t+1)＝xi(t)+vi(t+1)

48、其中，t为当前迭代次数，i＝1,2,3,...,n为粒子的编号；c1为个体学习因子、c2为社会学习因子、r1和r2这四个参数为在区间[0,1]内的随机数，pbest为第i个粒子的最优适应度值所在的位置，gbest为全局最优适应度值的粒子所在位置，τ为惯性权重系数，其迭代方程为：

49、τ＝τmax-τkt

50、其中，

51、4.2.2)对粒子群的速度、位置进行更新后，利用步骤3)的目标函数表达对每个粒子的适应度值进行更新，并记录每个粒子的更新情况；如果其中一个粒子的适应度值在迭代后没有更优，则该粒子对应的计数器会增加，如果其中一个粒子的适应度值在迭代后得到了优化，则该粒子对应的计数器将被重置为零；粒子计数器的更新方法如下：

52、

53、其中，count(i)为第i个粒子的计数器，xi_new为第i个粒子的新位置，f(·)为适应度值计算函数。

54、进一步的，所述步骤5的具体过程如下：

55、5.1)将所有目标按照目标靶点和进针点的绝对距离按照从大到小进行排序，这个顺序为对每个目标靶点进行路径规划的顺序，距离进针点越远的靶点将会被优先穿刺；

56、5.2)穿刺针将从初始穿刺路径的终点处开始对第二个目标靶点进行路径规划操作，如果在初始穿刺路径的终点处无法成功规划出到达第二个目标靶点的穿刺路径，那么针尖将退回到上一个停车转向点的位置；再次进行对第二个目标靶点的穿刺路径规划，以此类推，直到成功规划出到达第二个目标靶点的穿刺路径；如果在针尖退回到进针点之前没有成功规划出到达第二个目标靶点的路径，则从进针点重新开始路径规划；

57、根据上述进针策略完成对所有目标靶点的穿刺路径规划。

58、通过采用上述技术，与现有技术相比，本发明的有益效果如下：

59、本发明从整体的角度出发，提出一种新的基于智能粒子群算法的路径规划方案，规划方案针对穿刺过程的穿刺误差、路径长度以及与组织的碰撞函数建立相关的目标优化函数，并且结合了智能粒子群算法做进一步优化；能够克服传统规划方法缺陷，显著提高了穿刺效率以及对复杂环境的适应性和精度。