1.本发明涉及理疗机器人技术领域，更具体的说是涉及一种负压理疗机器人理疗力度及理疗路径的自适应调节方法。
背景技术：
2.目前的负压理疗机器人以负压理疗终端为主，仍然需要用人工手持设备来完成理疗过程，该过程需要人工长时间去操作设备，不仅容易产生疲劳感，还会由于人工受手法力度精度的差异，导致用户的体验度也不一样，很难保证理疗水平一致，从而产生不一样的效果。同时，由于理疗部位的差异，工作人员需要不断手动去调整负压理疗设备的力度、路径、温度等设备参数，其通信时间单位都是秒级，导致响应慢，用户体验差。
3.因此，如何实现负压理疗力度和理疗路径的自动调节是本领域技术人员亟需解决的问题。
技术实现要素：
4.有鉴于此，本发明提供了一种负压理疗机器人理疗力度及理疗路径自适应调节方法，在负压理疗时，实现对理疗路径和理疗力度的自动调节，并提高对理疗路径和力度补偿的响应速度。
5.为了实现上述目的，本发明采用如下技术方案：
6.一种负压理疗机器人理疗力度及理疗路径自适应调节方法，包括以下步骤：
7.周期性采集理疗目标轮廓，计算理疗目标点与机器人理疗头之间的距离，根据已标定的基准距离计算机器人理疗头在笛卡尔空间中的偏移量，根据该偏移量纠正机器人的理疗路径；
8.周期性采集机器人理疗头与理疗目标点间的接触力或力矩，将机器人的力控制问题转换为阻抗控制问题，通过自适应调节阻抗的刚度系数，调整理疗头作用于理疗目标点的作用力大小。
9.进一步的，所述负压理疗机器人通过机械臂控制理疗头移动，且理疗头安装在机械臂末端，阻抗控制采用如下公式：
10.；
11.；
12.其中，表示刚度系统，且，k
px
、k
py
、k
pz
、k
ax
、k
ay
、k
az
分别表示刚度系统k在不同坐标系下的刚度系数，下角标p表示位置，下角标a
表示姿态；表示理疗头作用于理疗目标点所需的作用力大小，且，δpx、δpy、δpz表示在x,y,z方向偏移量，δax、δay、δaz表示在x,y,z空间中，位置变化的方向变化；f表示接触力并转化到工具坐标的等效力，，f
x
、fy、fz表示在x,y,z方向力的大小，t
x
、ty、tz表示力在空间中方向的变化。
13.进一步的，刚度系数k的自适应调节方法为：
14.当时，刚度系数的计算公式为：
[0015][0016]
当时，刚度系数；
[0017]
其中，表示最大允许接触力或力矩；表示实际接触力或力矩。
[0018]
进一步的，该方法还包括：
[0019]
将力传感器检测的接触力或力矩转换到末端理疗头表面坐标系，计算实际的接触力。
[0020]
进一步的，假设力传感器中心所在的坐标系为{s}，末端理疗头表面中心的坐标系为{t},即工具坐标系，力传感器检测的相应力的转换公式为：
[0021][0022]
其中，t
ts
表示坐标系{s}相对于坐标系{t}的齐次变换，n、o、a分别是{s}坐标系的远点相对于{t}坐标系的值，下角标x、y、z分别是对应xyz坐标系下的轴，px、py、pz代表两个坐标系的旋转姿态。
[0023]
进一步的，末端理疗头的接触力或力矩为ft，其中，，f
tx
、f
ty
和f
tz
分别表示沿坐标系{t}的x、y和z轴方向的力，t
tx
、t
ty
和t
tz
分别表示绕坐标系{t}的x、y和z轴方向的力矩；
[0024]
传感器检测到的力或力矩为fs，其中，，f
sx
、f
sy
和f
sz
分别表示沿坐标系{s}的x、y和z轴方向的力，t
sx
、t
sy
和t
sz
分别表示绕坐标系{s}的x、y和z轴方向的力矩；
[0025]
；
[0026]
；
[0027]
；
[0028]
；
[0029]
；
[0030][0031]
其中，m
tx
、m
ty
、m
tz
分别表示x,y,z方向实际接触的力。
[0032]
进一步的，将理疗头朝向理疗目标点的移动方向定义为z方向，此时不需要绕工具坐标系{t}的旋转力，对其他四个自由度方向进行力控制，力控制公式为：
[0033][0034][0035]
其中，表示z方向的移动距离；表示步长；、分别表示沿x和y方向移动的位置；、分别表示绕x和y方向旋转的角度；、、和分别表示相应的力或力矩；、、和表示刚度系数。
[0036]
进一步的，机器人的理疗路径的纠正过程包括：
[0037]
通过机器人视觉系统周期性采集理疗目标轮廓，并对当前周期采集的理疗目标点与机器人理疗头之间的距离进行交比不变量的条件判定，若满足条件，则对理疗头进行趋近控制，若不满足条件，则对理疗头进行调整控制。
[0038]
进一步的，对理疗头进行调整控制的过程包括：
[0039]
根据已标定的基准距离计算理疗理疗头在笛卡尔空间下的一个位置偏移量；
[0040]
将笛卡尔空间下的位置偏移量转换成关节空间中的关节角度，对机械臂关节进行位置控制、速度控制和电流控制，使机器人的运动轨迹发生偏移；
[0041]
运动完成后的关节角度经过正运动学计算，得到笛卡尔空间下的机器人位姿；
[0042]
将调整后的机器人位姿反馈至机器人视觉系统，作为下一运动周期的初始数据。
[0043]
进一步的，对机器人的理疗路径进行纠正和/或对理疗头作用力进行调整时，将机器人视觉系统产生的关节角补偿值和/或机器人力觉系统产生的关节角补偿值直接补偿在关节空间插值环节。
[0044]
经由上述的技术方案可知，与现有技术相比，本发明具有以下有益效果：
[0045]
1、本发明把外部环境等效为导纳，而将机器人机械臂末端等效为阻抗，使机器人的力控制问题转变为阻抗调节问题。由于在负压理疗过程中，机器人与理疗目标表面之间的相对运动速度很小，所以不考虑加速度项，又由于所采用的机械臂是位置控制方式，所以只保留阻抗控制公式中的位置项，对阻抗控制公式进行简化，从而使机械臂控制起来更为简单、可靠。
[0046]
2、本发明结合实际的接触力或力矩，对阻抗控制公式的刚度系数进行自适应调整，进而实现对理疗力度的适应性调整，保证理疗过程的舒适性。
[0047]
3、本发明还对机器人机械臂位姿进行实时纠正，确保理疗路径的准确性，保证理疗效果。
[0048]
4、本发明在理疗过程中，将机器人视觉系统产生的关节角补偿值和/或机器人力觉系统产生的关节角补偿值直接补偿在关节空间插值环节，避免了笛卡尔空间插值环节，可提高视觉和力觉补偿的响应速度。
附图说明
[0049]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
[0050]
图1为本发明提供的负压理疗机器人理疗力度及理疗路径自适应调节方法的结构示意图；
[0051]
图2为本发明提供的负压理疗机器人的结构示意图；
[0052]
图3为本发明提供的阻抗控制结构图；
[0053]
图4为本发明提供的传感器检测的力转换示意图；
[0054]
图5为本发明提供的机器人视觉系统对理疗路径进行纠正的流程图；
[0055]
图6为传统的机器人插值流程示意图；
[0056]
图7为本发明提供的机器人视觉系统和机器人力觉系统的在关节空间的插值流程示意图。
具体实施方式
[0057]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0058]
实施例一
[0059]
如图1所示，本发明实施例公开了一种负压理疗机器人理疗力度及理疗路径自适应调节方法，包括以下步骤：
[0060]
周期性采集理疗目标轮廓，计算理疗目标点与机器人理疗头之间的距离，根据已标定的基准距离计算机器人理疗头在笛卡尔空间中的偏移量，根据该偏移量纠正机器人的理疗路径；
[0061]
周期性采集机器人理疗头与理疗目标点间的接触力或力矩，将机器人的力控制问题转换为阻抗控制问题，通过自适应调节阻抗的刚度系数，调整理疗头作用于理疗目标点的作用力大小。
[0062]
本发明实施例中的负压理疗机器人整体结构如图2所示，机体、移动底座、机械臂、六维力传感器、理疗头组件、负压装置、视觉定位装置和交互装置。机械臂为六自由度机械臂，其包括依次连接的多个关节臂，多个关节臂中的始级关节臂与机体前侧上部的关节臂安装座连接；理疗头组件包括原件支座，原件支座安装在筒形外壳结构的内部，其上端安装
有红外加热器和位移传感器，下端安装有末端理疗头；负压装置中的真空泵安装于机体内部，并与理疗设备控制器主机中集成的真空泵控制器电连接；与真空泵相连接的负压波纹管穿出机体之外，并与筒形外壳结构上的开孔相连接；负压装置中的气体流通路径为经末端理疗头流至筒形外壳结构上部的开孔，再流至负压波纹管内；视觉定位装置为安装在末级关节臂的自由端上的摄像头，实现机械臂相对于人体的坐标定位。
[0063]
实施例二
[0064]
本实施例对理疗力度进行自适应调节的过程进行了详细说明，具体如下：
[0065]
机器人力觉系统的阻抗控制利用norton等效网络概念，把外部环境等效为导纳，而将机器人机械臂末端等效为阻抗，这样机器人的力控制问题便变为阻抗调节问题。阻抗由弹簧－阻尼－惯量三项组成，期望力为：
[0066][0067]
其中，,为名义位置，x为实际位置。
△
x为位置误差，为速度偏差，为加速度偏差；k、b、m为弹性、阻尼和惯量系数矩阵，一旦k、b和m被确定，则可得到笛卡儿坐标的期望动态响应。
[0068]
如图3所示，为阻抗控制的控制框图，其中，xd是机器人原先规划好的位置，j是一个矩阵，经过j后，xd的偏移量就会从笛卡尔空间转变成关节空间的角度，k是刚性系数，设置好的一个定量。x是力矩传感器参与下，计算出的机器人实际位置；xe是机器人实际到达的位置，因为实际到达与理论值x是存在偏差的，一般不会超过0.003mm。是刚度反馈矩阵，阻尼反馈矩阵。
[0069]
当阻尼反馈矩阵时，称为刚度控制。刚度控制是用刚度矩阵k
p
来描述机器人末端作用力与位置误差的关系，即：
[0070][0071]
式中，kp通常为对角阵，即。刚度控制的输入为末端执行器在直角坐标中的名义位置，力约束则隐含在刚度矩阵k
p
中，调整k
p
中对角线元素值，就可改变机器人的顺应特性。
[0072]
阻尼控制则是用阻尼矩阵kv来描述机器人末端作用力与运动速度的关系，即：
[0073][0074]
式中kv是六维的阻尼系数矩阵，阻尼控制由此得名。通过调整kv中元素值，可改变机器人对运动速度的阻尼作用。
[0075]
阻抗控制本质上还是位置控制，因为其输入量为末端执行器的位置期望值（对刚度控制而言）和速度的期望值（对阻抗控制而言）。但由于增加了力反馈控制环，使其位置偏差和速度偏差与末端执行器与外部环境的接触力的大小有关，从而实现力的闭环控制。这里力－位置和力－速度变换是通过刚度反馈矩阵和阻尼反馈矩阵来实现的。这样系统的闭环刚度可求出：
[0076]
当时
[0077][0078]
[0079]kcp
是实际刚度矩阵，k
p
是理论刚度矩阵；
[0080]
当时
[0081][0082][0083]kcv
是实际阻尼矩阵，kv是理论阻尼矩阵。
[0084]
阻抗控制算法的思想是把外部环境等效为导纳，将机械臂本体末端等效为阻抗，这样就把力控制问题变为阻抗调节问题。阻抗控制的优点是使机械臂控制起来简单、可靠。但是要确定阻抗参数项并不容易，尤其是当负载不同时，系统的刚度系数、阻尼系数等都是不同的。
[0085]
本发明实施例通过分析负压理疗任务，当需要力控制时，操作对象（机器人机械臂）和被操作对象（理疗目标表面）之间的相对运动速度很小，所以可以不用考虑加速度项。接下来分析位置项和速度项。位置项难点在于确定刚度系数；速度项的难点在于确定弹性系数。不管是刚度系数还是弹性系数，都是把控制对象等效为弹性构件，通过调整弹性系数，以及和来调节系统的力，因此从根本上来讲，它们的作用效果是一样的。取位置项或速度项其中一项即可。而位置项或速度项，主要取决与机械臂的控制方式，即位置控制或速度控制。由于本发明负压理疗机器人通过机械臂控制理疗头移动，且理疗头安装在机械臂末端，所采用的机械臂是位置控制方式，所以取前面的位置项，则阻抗控制可以简化为如下公式：
[0086]
；
[0087]
；
[0088]
其中，表示刚度系统，且，k
px
、k
py
、k
pz
、k
ax
、k
ay
、k
az
分别表示刚度系统k在不同坐标系下的刚度系数，下角标p表示位置，下角标a表示姿态；表示理疗头作用于理疗目标点所需的作用力大小，且，δpx、δpy、δpz表示在x,y,z方向偏移量，δax、δay、δaz表示在x,y,z空间中，位置变化的方向变化；f表示接触力并转化到工具坐标的等效力，，f
x
、fy、fz表示在x,y,z方向力的大小，t
x
、ty、tz表示力在空间中方向的变化。
[0089]
其中，k代表了一个系统的刚度，值越大，表示刚性越强，即响应越快，响应快的表现就是加速度非常大，且容易出现超调现象；值越小，表示刚性越弱，即响应慢，加速度比较平缓，但响应时间有所增加。
[0090]
具体来说，刚度系数k的确定可以通过实验取得，但是当不同负载操作时，系统的刚度很难确定。本发明提出一种刚度系数k的自适应调节方法，具体如下：
[0091]
当时，刚度系数的计算公式为：
[0092][0093]
当时，刚度系数；
[0094]
其中，表示最大允许接触力或力矩；表示实际接触力或力矩。
[0095]
更有利的，本发明理疗机器人的六维力传感器安装在机械臂末端，末端效应器（即理疗头）则连接在传感器上，实现对接和力控制，相应的示意图如图4所示。可以看出，力传感器检测到的力并不是真实的末端效应器表面的接触力，而是通过末端效应器传递到传感器的力。因此，需要把传感器检测到的力和力矩转换到真实的接触力表面，以计算实际的接触力。具体转换过程如下：
[0096]
假设力传感器中心所在的坐标系为{s}，末端理疗头表面中心的坐标系为{t},即工具坐标系，力传感器检测的相应力的转换公式为：
[0097][0098]
其中，t
ts
表示坐标系{s}相对于坐标系{t}的齐次变换，n、o、a分别是{s}坐标系的远点相对于{t}坐标系的值，下角标x、y、z分别是对应xyz坐标系下的轴，px、py、pz代表两个坐标系的旋转姿态。
[0099]
；
[0100][0101][0102][0103][0104][0105]
其中，末端理疗头的接触力或力矩为f
t
，，f
tx
、f
ty
和f
tz
分别表示沿坐标系{t}的x、y和z轴方向的力，t
tx
、t
ty
和t
tz
分别表示绕坐标系{t}的x、y和z轴方向的力矩；
[0106]
传感器检测到的力或力矩为fs，其中，，f
sx
、f
sy
和f
sz
分别表示沿坐标系{s}的x、y和z轴方向的力，t
sx
、t
sy
和t
sz
分别表示绕坐标系{s}的x、y和z轴方向的力矩；
[0107]mtx
、m
ty
、m
tz
分别表示x,y,z方向实际接触的力。
[0108]
通过上述力的转换，就可以计算出实际的接触力。实际上，上述变化可以适用于任意坐标系之间的转换。
[0109]
在其他实施例中，将理疗头朝向理疗目标点的移动方向定义为z方向，z方向的移动是对接位置控制，即不断向目标点靠近，直到z方向检测到的力到大于等于设定的阈值。基于右手法则，可知，在装配过程中绕工具坐标系{t}旋转力实际上并不需要。其他四个自由度需要力控制，进行姿态调整，具体力控制公式为：
[0110][0111][0112]
其中，表示z方向的移动距离；表示步长；、分别表示沿x和y方向移动的位置；、分别表示绕x和y方向旋转的角度；、、和分别表示相应的力或力矩；、、和表示刚度系数。
[0113]
实施例三
[0114]
本实施例对机器人的理疗路径的纠正过程做了进一步说明，具体如下：
[0115]
对于机器人，视觉被公认为是最重要的感知能力。视觉控制器以图像处理得到的图像特征作为输入，输出执行机构的运动指令使各关节执行相应的运动，从而实现视觉控制。由图像处理得到的图像特征在图像像素坐标空间，而关节运动在三维坐标空间，因此视觉控制器需要建立起图像像素坐标空间与三维坐标空间之间的联系，从而得到执行器末端与理疗目标点的图像像素坐标差与三维空间中位置差的关系，得到下一周期的期望位姿之后，即可根据执行机构的逆运动学求得各关节的运动量，因此视觉控制器可通过摄像机透视投影模型和机械臂的运动学模型建立。
[0116]
对于机器人视觉系统，视觉反馈获得的信息可以是3维空间坐标，也可以是2维图像平面的图像特征，据此可以将系统分为基于位置的视觉伺服系统和基于图像的视觉伺服系统。在基于位置的控制结构中，视觉处理输出的是运动目标的坐标，并由此估计目标与机器人之间的相对位姿，以控制机器人在直角坐标空间中的运动，它将视觉处理与机器人运动控制分开，可以直观地在直角坐标系中描述期望的相对轨迹。尤其当运动目标的轨迹易于用直角坐标表达时，多采用这种结构。但基于位置的控制系统由于需要求解逆运动学方程，计算量比较大，同时它对机器人及摄像机等的标定误差比较敏感，其控制的精度直接依赖于系统模型、标定误差等方面。
[0117]
本发明采用闭环的视觉控制方式，如下：
[0118]
通过机器人视觉系统周期性采集理疗目标轮廓，计算理疗目标点与机器人理疗头之间的距离（和深度相机测距原理相同），并对当前周期采集的理疗目标点与机器人理疗头之间的距离进行交比不变量的条件判定，若满足条件，则对理疗头进行趋近控制，若不满足条件，则对理疗头进行调整控制，并将实时调整结果反馈回机器人视觉系统。
[0119]
具体来说，如图5所示，对理疗头进行调整控制的过程包括：
[0120]
根据已标定的基准距离计算当前周期理疗理疗头在笛卡尔空间下的一个位置偏移量；
[0121]
将笛卡尔空间下的位置偏移量转换成关节空间中的关节角度，对机械臂关节进行位置控制、速度控制和电流控制，使机器人的运动轨迹发生偏移；
[0122]
运动完成后的关节角度经过正运动学计算，得到笛卡尔空间下的机器人位姿；
[0123]
将调整后的机器人位姿反馈至机器人视觉系统，作为下一运动周期的初始数据。
[0124]
影响机器人下一个周期的运动（这里的机器人周期非常短暂，一般都是4~10ms作为一个周期），整个过程为闭环控制过程。
[0125]
在一个更有利的实施例中，如图6所示，传统的机器人插值分为两部分，即笛卡尔空间插值和关节空间插值。笛卡尔空间插值指的是将用户输入的目标点在笛卡尔空间插值生成细密的几何空间点，这些空间点再通过机器人逆运动学反解出对应的关节角，继而在关节空间中将这些关节角再次细化插值，生成能够满足伺服电机使用条件的数据。
[0126]
本发明实施例为了提高视觉和力觉补偿的响应速度，对机器人的理疗路径进行纠正和/或对理疗头作用力进行调整时，将机器人视觉系统产生的关节角补偿值和/或机器人力觉系统产生的关节角补偿值直接补偿在关节空间插值环节，具体如图7所示。图7中的pose表示位置，dpose0，dpose1，
…
dposen，代表一个插值周期的插值，force和visal分别表示视觉系统和力觉系统计算的关节补偿值，直接作用于关节插值流程，行成dq，即机器人实际运动位置。
[0127]
具体来说，当机器人处于视觉控制模式时（即理疗路径纠正阶段），机器人主控制器对伺服驱动的输出为：
[0128]
dqnn_out = dqnn j-1
·
dpose
visal
[0129]
其中，dqnn-out代表伺服驱动的输出，dqnn代表机器人主控制器计算得到输出，j-1
·
dpose
visal
表示视觉系统计算的视觉补偿量。
[0130]
当机器人处于力觉控制模式时（即力觉自适应调整阶段），机器人主控制器对伺服驱动的输出为：
[0131]
dqnn_out = dqnn j
t
·
dpose
force
[0132]
其中，dqnn-out代表伺服驱动的输出，dqnn代表机器人主控制器计算得到输出，j-1
·
dpose
visal
表示力觉系统计算的力觉补偿量。
[0133]
由于减少了笛卡尔空间插值环节，因此，本发明在进行理疗路径纠正和接触力调节时，末端执行器的响应速度会得到大幅提升。
[0134]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0135]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一
致的最宽的范围。