一种力反馈末端夹持器导纳控制方法

1.本发明涉及穿刺手术机器人末端夹持器设计及机器人柔顺控制的技术领域，尤其是指一种力反馈末端夹持器导纳控制方法。


背景技术：

2.随着机器人技术的不断发展与医学治疗手段的不断丰富，医疗机器人应运而生。医疗机器人是指各种应用于外科手术、康复治疗、残障人士辅助等医学用途的机器人设备，具有微创，精准，高效的特点。医疗机器人可以细分为手术机器人，康复机器人，辅助机器人等。其中穿刺手术机器人是手术机器人的一个重要分支，主要用于进行外科穿刺手术，如腰椎穿刺，血管穿刺，脏器穿刺等，对于相关疾病的诊断与恶性肿瘤的消融治疗有着重要作用。穿刺手术机器人一般由机械臂、空间定位系统、成像设施、工作站四大部分以及相应的软件组成。目前，穿刺手术机器人技术已经较为成熟，被广泛应用到临床中。机器人可辅助人工操作，降低穿刺手术难度，提高手术可靠性：所采用的光学导航系统可以辅助医生进行影像分析、路径规划，并对目标靶区进行精确定位与穿刺。手术机器人状态稳定，克服了传统手术易受医生个人操作和经验影响的缺点，提高手术的安全性和成功率。
3.然而，目前市场上的穿刺手术机器人仍然存在一定的局限性。目前成熟的穿刺手术机器人主要用于神经外科与骨科的穿刺手术，均无法对受呼吸运动影响的靶区，如肝，肺等进行精准的穿刺。因为这些部位的器官组织会随人体的呼吸运动发生位移和形变，从而导致目标靶区位置发生变化，降低系统的定位精度。此外，器官组织的生理运动还将给穿刺针造成挤压力，导致穿刺针发生弹性形变，偏离预定穿刺位置，导致穿刺失败。同时，器官组织与穿刺针之间的相互作用力还有可能对周围正常的人体器官组织造成损伤，甚至造成器官撕裂，降低手术的安全性，因此需设计可以适时调整的手术导航系统。仅依赖光学导航系统的穿刺机器人无法感知力信号并做出调整；并且由于穿刺针较长，仅使用一个三维力传感器并不能精准检测出穿刺针受力，因此需采用两个三维力传感器进行检测。目前，主流手术机器人系统和现行的穿刺手术技术均不能很好地解决上述问题。


技术实现要素：

4.本发明的目的在于克服现有技术的缺点与不足，提出了一种力反馈末端夹持器导纳控制方法，可以实现对穿刺手术机器人末端穿刺针的零力跟随控制，减少因组织生理运动对穿刺手术造成的干扰，提高穿刺精准度，减少对周边器官组织的损伤，从而提升手术的精准性与安全性。
5.为实现上述目的，本发明所提供的技术方案为：一种力反馈末端夹持器导纳控制方法，该方法需要用到带有下部引导端三维力传感器和上部夹持端三维力传感器的力反馈末端夹持器及穿刺针手术工具，所述力反馈末端夹持器固定于六自由度机械臂末端，所述穿刺针手术工具固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；该
方法包括以下步骤：
6.s1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的z轴与下部引导端三维力传感器的x轴和y轴构成组合坐标系oc；
7.s2、将穿刺针手术工具在组合坐标系oc下的受力[f
cx f
cy f
cz
]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差xw，再与初始位置xo进行叠加，求得下一时刻六自由度机械臂的期望位置xf；
[0008]
s3、结合六自由度机械臂的结构参数和dh表，将上述求得的六自由度机械臂的期望位置xf进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿。
[0009]
进一步，所述s1包括以下步骤：
[0010]
s101、根据六自由度机械臂的结构参数、位置信息和dh表，运用正向运动学求得六自由度机械臂基座坐标系下等效重力加速度gb和六自由度机械臂末端坐标系下等效重力加速度ge即力反馈末端执持器坐标系下等效重力加速度g
t
之间的转换关系：
[0011][0012]
其中，g
t
为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，r
be
表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
[0013]
其中，x
be
,y
be
,z
be
分别为绕x轴，y轴，z轴的旋转分量；为x
be
的x轴坐标，为x
be
的y轴坐标，是x
be
的z轴坐标；为y
be
的x轴坐标，为y
be
的y轴坐标，是y
be
的z轴坐标，为z
be
的x轴坐标，为z
be
的y轴坐标，是z
be
的z轴坐标；具体计算方法如下：
[0014][0015]
其中，θ1,θ2,θ3,θ4,θ5,θ6均为六自由度机械臂当前位置的关节角度；
[0016]
s102、将六自由度机械臂末端关节角度设置为10度，六自由度机械臂腕关节角度
设置为300度，六自由度机械臂腕关节运动步长设置为5度，六自由度机械臂腕关节角度依次减去运动步长，并赋值给六自由度机械臂腕关节角度；
[0017]
s103、将步骤s102求得的六自由度机械臂腕关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
[0018]
s104、在步骤s103的基础上，将组合坐标系下采集到的力信号拼接成向量f
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量g
t
；
[0019]
s105、六自由度机械臂末端关节运动步长设置为5度，六自由度机械臂腕关节角度依次加上运动步长，并赋值给六自由度机械臂末端关节角度；
[0020]
s106、将步骤s105求得的六自由度机械臂末端关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
[0021]
s107、结合步骤s103和s106，将组合坐标系下采集到的力信号拼接成向量f
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量g
t
；
[0022]
s108、通过步骤s107求得的f
t
和g
t
，计算能够得力反馈末端执持器自身的重力干扰值的等效质量m
t
为：
[0023][0024]
其中，为g
t
的转置向量；
[0025]
s109、根据步骤s108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值g为：
[0026][0027]
进一步，所述s2包括以下步骤：
[0028]
s201、对于导纳控制器，其运动模型为：其中m为惯性系数对角矩阵，b为阻力系数对角矩阵，k为刚度系数对角矩阵，fc为所受外力，xw为期望位置与实际位置之差，分别为xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在z轴与y轴检测到的横向力作为f
cy
,f
cz
，力反馈末端夹持器上方引导端三维力传感器在x轴检测到的力为f
cx
，此时fc＝[f
cx f
cy f
cz
]；
[0029]
s202、零力矫正姿态与当前姿态的差值δq作为位置控制器的输入信号，位置控制器根据δq对六自由度机械臂各关节的转角进行控制直至δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置x；
[0030]
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的六自由度机械臂的末端运动速度有：
[0031]
[0032][0033]
其中，j(q)为机器人雅可比矩阵，为六自由度机械臂末端的运动速度向量，为六自由度机械臂各关节运动速度向量，当中用表示第m个关节的空间位置对第n个关节角度的偏导数，m、n＝1,2,3,4,5,6；当前空间位置x与六自由度机械臂末端运动速度作为两个反馈信号输入到导纳控制器中；
[0034]
s203、导纳控制是输入力fc，输出期望位置与实际位置之差xw；由能够推导出方程能够推导出方程为xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令yw为xw的一阶微分，即六自由度机械臂的末端运动速度，为yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
[0035][0036]
其中，m-1
为m的逆矩阵；
[0037]
s204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格—库塔方法，求解步进值为δt和δt/2时两方程的四阶斜率估计值an、bn、cn、dn、en、fn、gn、hn，代入下式：
[0038][0039]
为xw在n+1时刻的迭代值；和分别为的一阶微分；由此求出某一时刻的期望位置与实际位置之差xw，并与初始位置xo进行叠加，进而求得期望位置xf；
[0040]
s205、应用导纳控制器的运动模型时，需确定模型的m、b、k的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
[0041][0042]
其中，ξ为阻尼比，惯性系数对角矩阵m的数值越大，响应越难发生突
变，即作用力发生变化时，六自由度机械臂无法及时发生改变；m小于阈值则会导致六自由度机械臂易受微小扰动影响而不稳定；弹性系数对角矩阵k的数值越大，则六自由度机械臂的回弹现象越严重，难以实现零力补偿的效果；阻尼系数对角矩阵b有利于减弱反弹现象和抑制响应突变；通过在实际操作中不断调整这三个矩阵的参数，能够控制六自由度机械臂达到期望的零力补偿状态。
[0043]
进一步，所述s3包括以下步骤：
[0044]
s301、结合六自由度机械臂的结构参数、六自由度机械臂的位置信息和dh表将s2求得的下一时刻六自由度机械臂的期望位置xf通过逆向运动学脚本函数控制机械臂实时求得六自由度机械臂的关节角度信息q；
[0045]
s302、通过机械臂通讯协议实时传输六自由度机械臂的位置信息，并和目标位置信息进行作差运算，若差值的绝对值小于0.5，表示六自由度机械臂到达目标位置，否则一直通过机械臂的脚本函数控制机械臂运动。
[0046]
本发明与现有技术相比，具有如下优点与有益效果：
[0047]
1、本发明采用的自重偏置滤除算法有效滤除了夹持器自重对力信号采集的干扰，提高穿刺时针体受力采集的准确度。
[0048]
2、本发明可以实现对穿刺手术机器人末端穿刺针的零力跟随控制。机器人可根据穿刺针受力情况自动进行受力补偿，调节姿态角度，从而避免穿刺针受到挤压发生弹性形变而偏离规划路径，进而提高穿刺精度。
[0049]
3、零力跟随控制可减弱穿刺针与靶区周边组织的相互作用力，减少针体与器官组织之间的相互拉扯，避免对正常组织造成损伤，提高了手术的安全性。
附图说明
[0050]
图1为本发明方法的逻辑流程示意图。
[0051]
图2为本发明方法的实际应用场景图。
具体实施方式
[0052]
下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
[0053]
参见图1和图2所示，本实施例所提供的力反馈末端夹持器导纳控制方法，需要用到带有下部引导端三维力传感器1和上部夹持端三维力传感器2的力反馈末端夹持器及穿刺针手术工具4，所述力反馈末端夹持器固定于六自由度机械臂3末端，所述穿刺针手术工具4固定在力反馈末端夹持器上，用于检测外界施加的作用力，当穿刺针手术工具4受力时，力反馈末端夹持器上的两个三维力传感器能够实时采集到相应的力信号；其具体包括以下步骤：
[0054]
s1、设计自重偏置滤除算法，该算法是以六自由度机械臂基座坐标系为基准，通过六自由度机械臂的运动排除力反馈末端执持器的重力干扰；在力反馈末端执持器的重力干扰滤除后，取上部夹持端三维力传感器的z轴与下部引导端三维力传感器的x轴和y轴构成组合坐标系oc，具体包括以下步骤：
[0055]
s101、根据六自由度机械臂的结构参数、位置信息和dh表，运用正向运动学求得六
自由度机械臂基座坐标系下等效重力加速度gb和六自由度机械臂末端坐标系下等效重力加速度ge即力反馈末端执持器坐标系下等效重力加速度g
t
之间的转换关系：
[0056][0057]
其中，g
t
为力反馈末端执持器的等效重力加速度，ge为六自由度机械臂末端的等效重力加速度，gb为六自由度机械臂基座的等效重力加速度，r
be
表示六自由度机械臂基座坐标系和六自由度机械臂末端坐标系之间的转换关系；
[0058]
其中，x
be
,y
be
,z
be
分别为绕x轴，y轴，z轴的旋转分量；为x
be
的x轴坐标，为x
be
的y轴坐标，是x
be
的z轴坐标；为y
be
的x轴坐标，为y
be
的y轴坐标，是y
be
的z轴坐标，为z
be
的x轴坐标，为z
be
的y轴坐标，是z
be
的z轴坐标；具体计算方法如下：
[0059][0060]
其中，θ1,θ2,θ3,θ4,θ5,θ6均为六自由度机械臂当前位置的关节角度；
[0061]
s102、将六自由度机械臂末端关节角度设置为10度，六自由度机械臂腕关节角度设置为300度，六自由度机械臂腕关节运动步长设置为5度，六自由度机械臂腕关节角度依次减去运动步长，并赋值给六自由度机械臂腕关节角度；
[0062]
s103、将步骤s102求得的六自由度机械臂腕关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
[0063]
s104、在步骤s103的基础上，将组合坐标系下采集到的力信号拼接成向量f
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量g
t
；
[0064]
s105、六自由度机械臂末端关节运动步长设置为5度，六自由度机械臂腕关节角度依次加上运动步长，并赋值给六自由度机械臂末端关节角度；
[0065]
s106、将步骤s105求得的六自由度机械臂末端关节角度通过六自由度机械臂脚本函数控制六自由度机械臂运动至相应位置；
[0066]
s107、结合步骤s103和s106，将组合坐标系下采集到的力信号拼接成向量f
t
，将每一时刻力反馈末端执持器的等效重力加速度g
t
拼接成向量g
t
；
[0067]
s108、通过步骤s107求得的f
t
和g
t
，计算能够得力反馈末端执持器自身的重力干扰
值的等效质量m
t
为：
[0068][0069]
其中，为g
t
的转置向量；
[0070]
s109、根据步骤s108求得的力反馈末端执持器自身的重力干扰值的等效质量，能够求得力反馈末端执持器自身的重力干扰值g为：
[0071][0072]
s2、将穿刺针手术工具在组合坐标系oc下的受力[f
cx f
cy f
cz
]输入到基于位置控制的导纳控制器中，获得期望位置与实际位置之差xw，再与初始位置xo进行叠加，求得下一时刻六自由度机械臂的期望位置xf，具体包括以下步骤：
[0073]
s201、对于导纳控制器，其运动模型为：其中m为惯性系数对角矩阵，b为阻力系数对角矩阵，k为刚度系数对角矩阵，fc为所受外力，xw为期望位置与实际位置之差，分别为xw的一阶微分与二阶微分；取力反馈末端夹持器下方引导端三维力传感器在z轴与y轴检测到的横向力作为f
cy
,f
cz
，力反馈末端夹持器上方引导端三维力传感器在x轴检测到的力为f
cx
，此时fc＝[f
cx f
cy f
cz
]；
[0074]
s202、零力矫正姿态与当前姿态的差值δq作为位置控制器的输入信号，位置控制器根据δq对六自由度机械臂各关节的转角进行控制直至δq趋向于零，输出此时的关节角度信息q，并利用正运动学f(q)来求解出当前空间位置x；
[0075]
接着，利用机器人的雅克比矩阵求解每一时刻姿态即关节角度信息q所对应的关节速度有：
[0076][0077][0078]
其中，j(q)为机器人雅可比矩阵，为六自由度机械臂末端的运动速度向量，为六自由度机械臂各关节运动速度向量，当中用表示第m个关节的空间
位置对第n个关节角度的偏导数，m、n＝1,2,3,4,5,6；当前空间位置x与六自由度机械臂末端运动速度作为两个反馈信号输入到导纳控制器中；
[0079]
s203、导纳控制是输入力fc，输出期望位置与实际位置之差xw；由能够推导出方程能够推导出方程为xw在n时刻的迭代值；对该方程进行降阶处理，采用变量替换的方法，令yw为xw的一阶微分，即六自由度机械臂的末端运动速度，为yw的一阶微分，即六自由度机械臂的末端运动加速度，有：
[0080][0081]
其中，m-1
为m的逆矩阵；
[0082]
s204、上式为一阶微分方程组，对方程进行离散化处理，采用龙格—库塔方法，求解步进值为δt和δt/2时两方程的四阶斜率估计值an、bn、cn、dn、en、fn、gn、hn，代入下式：
[0083][0084]
为xw在n+1时刻的迭代值；和分别为的一阶微分；由此求出某一时刻的期望位置与实际位置之差xw，并与初始位置xo进行叠加，进而求得期望位置xf；
[0085]
s205、应用导纳控制器的运动模型时，需确定模型的m、b、k的数值；对模型方程应用拉普拉斯变换，得到该模型的传递函数为：
[0086][0087]
其中，ξ为阻尼比，惯性系数对角矩阵m的数值越大，响应越难发生突变，即作用力发生变化时，六自由度机械臂无法及时发生改变；m过小则会导致六自由度机械臂易受微小扰动影响而不稳定；弹性系数对角矩阵k的数值越大，则六自由度机械臂的回弹现象越严重，难以实现零力补偿的效果；阻尼系数对角矩阵b有利于减弱反弹现象和抑制响应突变；通过在实际操作中不断调整这三个矩阵的参数，能够控制六自由度机械臂达到期望的零力补偿状态。
[0088]
s3、结合六自由度机械臂的结构参数和dh表，将上述求得的六自由度机械臂的期望位置xf进行逆向运动学转换，求得六自由度机械臂期望的关节角度信息q，再通过脚本函数控制六自由度机械臂到达期望位置，从而实现穿刺针手术工具的零力补偿，具体包括以下步骤：
[0089]
s301、结合六自由度机械臂的结构参数、六自由度机械臂的位置信息和dh表将s2求得的下一时刻六自由度机械臂的期望位置xf通过逆向运动学脚本函数控制机械臂实时求得六自由度机械臂的关节角度信息q；
[0090]
s302、通过机械臂通讯协议实时传输六自由度机械臂的位置信息，并和目标位置
信息进行作差运算，若差值的绝对值小于0.5，表示六自由度机械臂到达目标位置，否则一直通过机械臂的脚本函数控制机械臂运动。
[0091]
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。