本发明属于医疗设备，具体是一种用于前列腺手术的辅助穿刺装置及逆运动学求解方法。
背景技术：
1、前列腺癌是男性泌尿生殖系统最常见的恶性肿瘤，穿刺活检是鉴别前列腺癌的重要手段。目前，穿刺活检主要有两种方式:传统手动穿刺和机械手臂辅助穿刺。传统手动穿刺主要依赖医生的经验判断穿刺方向和深度，穿刺点难以把握，同时还会因手持穿刺针导致的抖动增加了误伤周围组织的风险，容易对患者造成二次伤害。机械手臂辅助穿刺能够增加穿刺过程的稳定性,提高穿刺精度和成功率，降低手术风险。然而，现有的辅助穿刺机械手臂通常受限于自身机械结构，末端活动范围较小，难以全面覆盖穿刺区域，而且操作灵活性不足。
2、公告号为cn116019558b的发明专利公开了一种电磁导航穿刺机器人系统，采用工业六轴机械臂保证了定位精度，但是工业机械臂体积较大操作不灵活，同时需要多轴配合实现末端退针。公告号为cn106901836b的发明专利公开了一种前列腺医疗穿刺机器人，能够灵活调节穿刺角度，但是实际穿刺点与目标穿刺位置容易产生偏差，穿刺精度无法保证。公开号为cn114287997a的专利申请公开了一种医疗穿刺机器人，末端采用齿轮来实现穿刺角度的高精度控制，可以在穿刺平面内以不同角度入针，但是可达的空间范围较小。此外，现有逆运动学求解方法都是直接通过穿刺针末端位姿得到各个关节角度，存在参数多、求解困难等问题。
3、针对上述问题，本发明提出了一种用于前列腺手术的辅助穿刺装置及逆运动学求解方法。
技术实现思路
1、针对现有技术的不足，本发明拟解决的技术问题是，提供一种用于前列腺手术的辅助穿刺装置及逆运动学求解方法。
2、本发明解决所述技术问题采用如下的技术方案：
3、一种用于前列腺手术的辅助穿刺装置，包括超声探头进给模块和穿刺针定位进给模块；所述穿刺针定位进给模块包括支撑基座、伺服电机一、连接臂一、伺服电机二、连接臂二、伺服电机三、伺服电机四、连接臂三、连接臂四、旋转关节一、旋转关节二和丝杠模组；其中，伺服电机一位于支撑基座上且输出轴平行于z轴，连接臂一的一端与伺服电机一的输出轴连接，另一端连接伺服电机二，伺服电机二的输出轴平行于y轴；连接臂二呈l形，一端与伺服电机二的输出轴连接，另一端连接伺服电机三，伺服电机三的输出轴平行于z轴；连接臂三的一端与伺服电机三的输出轴连接，另一端与伺服电机四连接，伺服电机四的输出轴平行于x轴；连接臂四的一端与伺服电机四的输出轴连接，另一端设有旋转关节一，旋转关节一能在水平面内往复摆动；旋转关节二位于旋转关节一上，旋转关节二能在竖直面内往复摆动，旋转关节二与旋转关节一的摆动中心共线；丝杠模组安装在旋转关节二上，穿刺针通过穿刺针安装座安装在丝杠模组的滑块上，穿刺针轴线与两个旋转关节摆动中心所在直线交于一点，该点即为换向点，其位置在穿刺针末端位置变换过程中始终保持不变。
4、进一步的，上述装置的逆运动学求解过程为：
5、第一步：获取穿刺针末端的目标位置(x,y,z)，根据式(1)～(3)计算换向点的目标位置(x1,y1,z1)；
6、
7、式中，l是穿刺针长度，ψ、β和γ分别是穿刺针与xoy、yoz、xoz平面之间的夹角；
8、第二步：求解伺服电机一至伺服电机四的期望角度，根据期望角度控制伺服电机，使换向点到达目标位置；
9、相邻连杆之间的坐标系转换矩阵表示为：
10、
11、式中，表示由连杆i-1到连杆i的坐标系转换矩阵，i＝1，2，...，6；θi表示绕zi-1轴使xi-1轴旋转到xi轴的角度，αi表示绕xi轴使zi-1轴旋转到zi轴的角度，di表示xi-1与xi轴沿zi-1轴重合的长度，bi表示zi-1与zi轴沿xi轴重合的长度；
12、由正向运动学得到的换向点和穿刺针末端位姿分别为：
13、
14、式中，表示换向点位姿，表示穿刺针末端位姿；
15、由正向运动学得到的换向点和穿刺针末端位姿矩阵分别为：
16、
17、式中，{px1，py1，pz1}表示换向点位置，单位矢量{nx1，ny1，nz1}、{ox1，oy1，oz1}、{ax1，ay1，az1}表示换向点姿态；{px2，py2，pz2}表示穿刺针末端位置，单位矢量{nx2，ny2，nz2}、{ox2，oy2，oz2}、{ax2，ay2，az2}表示穿刺针末端姿态；
18、换向点位置的雅可比矩阵表示为：
19、
20、式中，jvi＝zi-1×(pω-pi-1)，jωi＝zi-1，i＝1，2，...，5；pω是换向点相对于基坐标系原点的位置矢量，pi-1是关节i-1坐标系原点相对于基坐标系原点的位置矢量，zi-1为绕关节i-1运动的单位矢量；
21、初始化角度向量θm＝(θ1 θ2 θ3 θ4)，θ5设置为定值0°，根据换向点位置的雅可比矩阵对换向点位置进行迭代求解，计算换向点当前位置与目标位置之间的误差，得到伺服电机一至伺服电机四的期望角度；
22、第三步：与求解换向点目标位置同理，求解穿刺针的偏航角θ5和俯仰角θ6，根据偏航角调节旋转关节一的摆动角度，根据俯仰角调节旋转关节二的摆动角度，使穿刺针末端到达目标位置。
23、进一步的，所述超声探头进给模块包括丝杠底座一、探头固定夹、滚珠丝杠二、滑块一和步进电机一；滚珠丝杠二的两端通过丝杠挡板安装在丝杠底座一上，滚珠丝杠二的一端穿过其中一个丝杠挡板与步进电机一的输出轴连接；滑块一滑动安装在滚珠丝杠二上，超声探头通过探头固定夹位于滑块一上，且超声探头能够相对于探头固定夹转动。
24、与现有技术相比，本发明具有如下优点：
25、1、将超声探头进给和穿刺针定位进给两部分采用分离式设计，使得操作更加灵活。穿刺针的进给运动采用滑动自由度，不仅穿刺更加便捷，而且完成穿刺后可沿原路径平稳退针，减少了退针时的抖动。该装置具有穿刺覆盖范围大、定位精度高等优势，可提高单次穿刺成功率。
26、2、现有技术都是根据穿刺针末端目标位姿来进行逆运动学求解得到各个关节角度，存在参数多、计算复杂、求解困难等问题，本发明通过换向点和穿刺针末端的目标位置来进行逆运动学求解，得到各个关节需要转动的角度，实现了穿刺点的精准定位，大大降低了末端位姿逆运动学求解的难度。
1.一种用于前列腺手术的辅助穿刺装置，其特征在于，该装置包括超声探头进给模块和穿刺针定位进给模块；所述穿刺针定位进给模块包括支撑基座、伺服电机一、连接臂一、伺服电机二、连接臂二、伺服电机三、伺服电机四、连接臂三、连接臂四、旋转关节一、旋转关节二和丝杠模组；
2.根据权利要求1所述的用于前列腺手术的辅助穿刺装置，其特征在于，所述装置的逆运动学求解过程为：
3.根据权利要求2所述的用于前列腺手术的辅助穿刺装置，其特征在于，在逆运动学求解过程中，将伺服电机一几何中心到伺服电机二几何中心等效为连杆一，伺服电机二几何中心到连接臂二中心轴线的拐点等效为连杆二，连接臂二中心轴线的拐点到伺服电机三顶端等效为连杆三，伺服电机三顶端到旋转关节一顶端等效为连杆四，旋转关节一顶端到旋转关节二底端等效为连杆五，旋转关节二底端到穿刺针末端等效为连杆六，伺服电机一至伺服电机四等效为关节一至关节四，旋转关节一等效为关节五，旋转关节二等效为关节六。
4.根据权利要求1～3任一所述的用于前列腺手术的辅助穿刺装置，其特征在于，所述超声探头进给模块包括丝杠底座一、探头固定夹、滚珠丝杠二、滑块一和步进电机一；滚珠丝杠二的两端通过丝杠挡板安装在丝杠底座一上，滚珠丝杠二的一端穿过其中一个丝杠挡板与步进电机一的输出轴连接；滑块一滑动安装在滚珠丝杠二上，超声探头通过探头固定夹位于滑块一上，且超声探头能够相对于探头固定夹转动。
