本发明涉及穿刺手术机器人领域，特别涉及一种穿刺手术机器人柔顺控制系统。
背景技术：
柔顺控制(随动控制)指机器人能够对外界环境有顺从的能力，大范围柔顺控制指其在所能迗到的所有构型下都能够实现柔顺运动。手术操纵者通过视觉确定机器人末端执行器所在的位置，和大脑中所期望的目标点进行比较并得到相应的位置偏差，然后使用手臂对机器人末端执行器施加力的作用，通过机器人腕部安装的力传感器得到外部的作用力，并反馈到控制器，使机器人能够实时跟随操纵者的意志进行运动。现在手术过程中，手术精密度要求越来越高，常规的肉眼手术已经不能满足高难度手术了，因此，急需一种工作精度高、工作稳定且效率高的穿刺手术机器人柔顺控制系统装置。
技术实现要素：
本发明要解决的问题在于提供一种工作精度高、工作稳定且效率高的一种穿刺手术机器人柔顺控制系统。
为解决上述问题，本发明提供一种穿刺手术机器人柔顺控制系统装置，包括：
基于微分控制器的pd控制模块，用于抑制测量噪声以及跟踪原始信号；
阻抗控制位置模型建立模块，用于建立机器人控制循环的动态运动模型；
力反馈控制模块，跟随操作者实现零力跟随控制即机械臂关节所受外力合力为0。
本发明的有益效果是，通过基于微分控制器的而改进的pd控制模块来实现对人体模型的实时跟踪效果，不仅能很好地对测量噪声进行抑制，还能够无超调的跟踪原始信号，实用性强，效率高，并且通过阻抗控制位置模型建立模块，使用内部位置控制器来实现跟踪理想阻抗模型的位置，降低了算法复杂度，提高了实时性。
进一步的，所述基于微分控制器的pd控制模块具体包括：
微分跟踪器的离散模型单元，用于建立机器人行程轨迹的运动数学关系；
逆运动学数学关系建立单元，用于将目标位置进行求解从而得出各个机器人的关节的控制量。
进一步的，所述阻抗控制位置模型建立模块具体包括：
位置控制模块单元，用于将具体空间位置通过逆运动学反解到关节空间中；
卡尔曼滤波器单元，用于对使用中产生的噪声进行抑制。
进一步的，所述位置控制模块单元还具体包括：
重力补偿算法单元，用于滤除末端执行器所造成的影响；
力衰减修正单元，用于控制偏差通过死区衰减后输入力控制器，从而得到修正值。
进一步的，所述力反馈控制模块包括力数字矩阵计算单元，用于将外部力通过计算反馈给控制器进行控制移动。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明一种穿刺手术机器人柔顺控制系统的柔顺控制的应用示意图；
图2是本发明一种穿刺手术机器人柔顺控制系统的基于微分跟踪器的pd控制原理框图；
图3是本发明一种穿刺手术机器人柔顺控制系统的大范围柔顺控制原理图；
图4是本发明一种穿刺手术机器人柔顺控制系统的位置跟踪曲线及其受力图；
图5是本发明一种穿刺手术机器人柔顺控制系统的柔顺控制接触力曲线图。
具体实施方式
下面结合具体实施例，对本发明的内容做进一步的详细说明：
本发明要解决的问题在于提供一种工作精度高、工作稳定且效率高的一种穿刺手术机器人柔顺控制系统。
如图1所示，为解决上述问题，本发明提供一种穿刺手术机器人柔顺控制系统装置，包括：
基于微分控制器的pd控制模块，用于抑制测量噪声以及跟踪原始信号；
阻抗控制位置模型建立模块，用于建立机器人控制循环的动态运动模型；
力反馈控制模块，跟随操作者实现零力跟随控制即机械臂关节所受外力合力为0。
本发明的有益效果是，通过基于微分控制器的而改进的pd控制模块来实现对人体模型的实时跟踪效果，不仅能很好地对测量噪声进行抑制，还能够无超调的跟踪原始信号，实用性强，效率高，并且通过阻抗控制位置模型建立模块，使用内部位置控制器来实现跟踪理想阻抗模型的位置，降低了算法复杂度，提高了实时性。
进一步的，所述基于微分控制器的pd控制模块具体包括：
微分跟踪器的离散模型单元，用于建立机器人行程轨迹的运动数学关系；
逆运动学数学关系建立单元，用于将目标位置进行求解从而得出各个机器人的关节的控制量。
如图2，图3所示，在实际操作中，微分跟踪器的离散模型单元的模型如下：
其中u(k)是在k时刻的控制变量；
x1是输入信号的跟踪信号；
x2为跟踪信号的近似微分信号；
t是米样时间；
h是滤波系数；
r是速度系数；
fst函数形式如下:
其中:
δ＝rh,δ0＝δh；
y＝x0-u hx2；
操纵者根据作业现场的情况判断出机器人所期望的目标点，并对机器人末端执行器施加力的作用，通过六维力传感器得到外部的作用力fs，将该信号通过死区衰减后输入微分跟踪器(td)，对力信号进行处理，得到力信号的跟踪信号和力信号的近似微分信号，分别输入给pd控制器中的p项和d项，并输出位置修正值x，和当前位置进行比较后得到笛卡尔空间目标位姿，最后对目标位姿进行逆运动学求解后将各关节的控制量输入机器人位置控制内环完成一个控制循环。
进一步的，所述阻抗控制位置模型建立模块具体包括:
位置控制模块单元，用于将具体空间位置通过逆运动学反解到关节空间中；
卡尔曼滤波器单元，用于对使用中产生的噪声进行抑制。
进一步的，所述位置控制模块单元还具体包括：
重力补偿算法单元，用于滤除末端执行器所造成的影响；
力衰减修正单元，用于控制偏差通过死区衰减后输入力控制器，从而得到修正值。
在实际操作中，阻抗控制位置模型建立模块如下：
fh＝md(xd-xc) bd(xd-xc) kd(xd-xc)；
xc表示当前位置，
xd表示期望位置，
md表示机器人的虚拟惯性矩阵，
bd表示机器人的虚拟阻尼矩阵，
kd表示机器人的虚拟刚度矩阵，
模型中的md，kd为机器人的阻抗特性系数，都为对角阵。md虚拟惯性矩阵对冲击力较强，速度变换较大的运动过程有较大影响；bd虚拟阻尼矩阵对外界干扰以及位置变化较快的运动有较大的影响；虚拟刚度矩阵对低速运动或静止状态附近的运动影响较大。
机器人所需位置可以在拉普拉斯域中表示为：
△x(s)＝fh(s)/mds2 bds kd＝fh(s)h(s)；
△x(s)是△x的拉普拉斯变换，fh(s)是fh和s的拉普拉斯变换。
通过上述分析,机器人关节空间的柔顺位置控制器为:
通过使用向后差分的方法得到速度与加速度的表达式：
△x(k)＝a0△f(k) a1△x(k-1) a2△x(k-2)；
其中：
如图4所示，经过此方式，在工作中，在追踪误差时候，当到达0.25秒之后，机器人已经能够跟踪上给定信号，并且随着时间推进，跟随误差缓缓减小，而所受到的外界接触力基本稳定在0.2n左右。
如图5所示，并且，在柔顺控制接触力曲线中可以发现在未施加外力的情况下，机器人计算得到的基座标系下的受力几乎等于0，机器人可以很好的保持稳定不会出现抖动的情况；而在外力拖动时，机器人受到的外界拖动力也维持在一个较低的水平(小于4n)，这说明机器人能够在任何位姿下都能达到快速的跟随，实现大范围的柔顺控制。
进一步的，所述力反馈控制模块包括力数字矩阵计算单元，用于将外部力通过计算反馈给控制器进行控制移动。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
1.一种穿刺手术机器人柔顺控制系统，其特征在于，包括：
基于微分控制器的pd控制模块，用于抑制测量噪声以及跟踪原始信号；
阻抗控制位置模型建立模块，用于建立机器人控制循环的动态运动模型；
力反馈控制模块，跟随操作者实现零力跟随控制即机械臂关节所受外力合力为0。
2.根据权利要求1所述的穿刺手术机器人柔顺控制系统，其特征在于，所述基于微分控制器的pd控制模块具体包括：
微分跟踪器的离散模型单元，用于建立机器人行程轨迹的运动数学关系；
逆运动学数学关系建立单元，用于将目标位置进行求解从而得出各个机器人的关节的控制量。
3.根据权利要求1所述的穿刺手术机器人柔顺控制系统，其特征在于，所述阻抗控制位置模型建立模块具体包括：
位置控制模块单元，用于将具体空间位置通过逆运动学反解到关节空间中；
卡尔曼滤波器单元，用于对使用中产生的噪声进行抑制。
4.根据权利要求3所述的穿刺手术机器人柔顺控制系统，其特征在于，所述位置控制模块单元还具体包括：
重力补偿算法单元，用于滤除末端执行器所造成的影响；
力衰减修正单元，用于控制偏差通过死区衰减后输入力控制器，从而得到修正值。
5.根据权利要求1所述的穿刺手术机器人柔顺控制系统，其特征在于，所述力反馈控制模块包括力数字矩阵计算单元，用于将外部力通过计算反馈给控制器进行控制移动。
技术总结