一种基于多传感器感知的艾灸机器人运动控制方法与流程

本发明涉及一种艾灸机器人的运动控制方法，特别涉及一种基于多传感器感知的艾灸机器人运动控制方法。

背景技术：

目前使用机器人协助医师进行艾灸治疗的研究正逐渐兴起，由于人体的体表呈曲面的，艾条与人体的体表之间的距离以及姿态都影响到艾灸的效果，所以在艾灸治疗时，艾灸机器人需要时刻根据人体体表曲面变化和灸疗穴位温度的变化进行姿态和位置调整，如专利号cn107791281a公布了一种四自由度的艾灸辅助机械臂，该机械臂基于视觉的方法进行寻穴，寻穴范围广。如专利号cn109620712a公布了一种艾灸机器人，该机器人能模拟各种人工施灸手法，并控制机器人在运动过程中跟随施灸部位皮肤温度和人体体表曲面变化做出调整。如专利号cn109718092a公开了一种基于关节机器人的艾灸系统及方法，该系统集成了距离传感器，温度传感器和视觉传感器等，能控制机器人夹持艾条进行灸疗。在灸疗时，机器人能够根据人体位置和姿态的变化做出调整，保证灸疗效果。然而，在公布的专利中虽有涉及控制机器人根据人体位置和姿态的变化做出调整的阐述，但并没有详述其控制方法。

技术实现要素：

本发明目的在于提供一种基于多传感器感知的艾灸机器人运动控制方法，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。

为解决上述技术问题所采用的技术方案：

一种基于多传感器感知的艾灸机器人运动控制方法，其采用关节机器人，所述关节机器人设置有执行末端和机器人控制器，在所述执行末端设置有温度传感器和多个距离传感器，艾灸机器人运动控制方法具体包括如下步骤：

步骤一：静态建模，控制所述关节机器人的执行末端在人体表面上方执行任意手法轨迹的运动，同时多个所述距离传感器扫描人体表面，并测量出执行末端与人体体表之间的距离值，机器人控制器根据所述距离传感器采集的距离数值拟合出体表的静态曲面模型并生成执行末端的主动运动轨迹程序；

步骤二：把灸头装设于执行末端上，根据所述主动运动轨迹程序，机器人控制器控制关节机器人上的执行末端运动；

步骤三：动态实时补偿，多个距离传感器实时测量执行末端与人体体表的距离值，并计算测量值和设定施灸的高度值之间的误差△eh；温度传感器实时监测人体体表穴位的温度，并计算测量值和设定的施灸温度值之间的误差△et，机器人控制器采用闭环控制算法拟合出执行末端的随动运动轨迹；

步骤四：机器人控制器把主动运动轨迹和随动运动轨迹采用多项式拟合在一起，最终形成执行末端的叠加运动轨迹。

本发明的有益效果是：在使用时，灸头装设于执行末端上，机器人控制器用于接收距离传感器、温度传感器的数值来调整灸头的位置和姿态，其实质上就是调整执行末端的位置和姿态，通过手法轨迹运动建立静态的准确的人体体表曲面模型和基于距离传感器、温度传感器的误差数值进行动态实时位置补偿，即静态建模和动态实时补偿共同作用，使得灸头可在人体体表保持恒定的距离进行运动，并保持保持灸头中心轴线和人体曲面的法线重合。

作为上述技术方案的进一步改进，在步骤一中，关节机器人的执行末端循环执行手法轨迹运动多次。

作为上述技术方案的进一步改进，在步骤一和步骤三中，执行末端与人体体表之间的距离值的具体测量方法如下：所述距离传感器的数量为四个，四个距离传感器呈十字对称设置于执行末端同一平面上，四个距离传感器包括相对设置的第一距离传感器和第三距离传感器、相对设置的第二距离传感器和第四距离传感器，第一距离传感器、第二距离传感器、第三距离传感器和第四距离传感器分别获取到距离数据d1、d2、d3、d4，第一距离传感器和第三距离传感器的轴线所组成的平面与关节机器人b角所处的平面重合，其中第二距离传感器和第四距离传感器的轴线所组成的平面与关节机器人c角所处的平面重合，所述第一距离传感器与第三距离传感器之间距离、第二距离传感器与第四距离传感器之间的距离均为l，使用三角余切公式：tanθ＝(d3-d1)/l可计算出关节机器人姿态的b角补偿值θ，同理使用d2、d4、l可计算出关节机器人姿态的c角补偿值α，使用d1、d2、d3、d4、θ和α的值，结合三角函数推导出来的公式得出当前执行末端的实际高度值(即是出执行末端与人体体表之间的距离值)：

h＝(d1+d2+d3+d4)/4)*cos(atan(sqrt(tan(90-θ)*tan(90-θ)+tan(90-α)*tan(90-α)))*π/180)。

作为上述技术方案的进一步改进，在步骤一中，多个距离传感器测量并计算得出的多组实际高度值h，机器人控制器将多组的高度值h拟合成曲线，并采用样条曲线插值的方式不断拟合优化，得出人体表面的曲线模型。

作为上述技术方案的进一步改进，在步骤一中，采用三次样条曲线插值的方式生成执行末端的主动运动轨迹，并对主动运动轨迹进行平滑处理，得出人体体表的静态曲面模型和关节机器人的主动运动程序。

作为上述技术方案的进一步改进，在步骤三中，距离传感器测量计算得出的实际距离值h减去设定的距离值hset，得到距离补偿值△eh，即h-hset＝△eh；关节机器人控制器实时计算△eh、θ、α，并根据其值进行实时的位置补偿。

作为上述技术方案的进一步改进，在所述执行末端安装有力矩传感器，力矩传感器用于监控执行末端是否与障碍物发生碰撞。

附图说明

下面结合附图和实施例对本发明做进一步的说明；

图1是本发明所提供的关节机器人，其一实施例的使用时的状态示意图；

图2是本发明所提供的多个距离传感器，其一实施例的测量示意图；

图3是本发明讲多个距离值h值拟合成曲线的示意图；

图4是本发明建立静态曲面模型时的流程图；

图5是本发明动态实时补偿执行末端位置的原理图；

图6是本发明的距离闭环控制原理图；

图7是本发明的姿态角度闭环控制原理图。

具体实施方式

本部分将详细描述本发明的具体实施例，本发明之较佳实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案，但其不能理解为对本发明保护范围的限制。

在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，如果具有“若干”之类的词汇描述，其含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。

本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。

参照图1至图7，本发明的基于多传感器感知的艾灸机器人运动控制方法，作出如下实施例：

本实施例的基于多传感器感知的艾灸机器人运动控制方法，采用关节机器人100，具体地，如图1所示，本实施例为6关节机器人。所述关节机器人100设置有执行末端和机器人控制器，在所述执行末端设置有温度传感器600和多个距离传感器，温度传感器600和距离传感器与机器人控制器信号连接，温度传感器600用于实时测量人体体表穴位的温度，具体地，距离传感器个数不少于4个，本实施例的距离传感器的数量为四个，四个距离传感器呈十字对称设置于执行末端同一平面上，四个距离传感器包括相对设置的第一距离传感器200和第三距离传感器300、相对设置的第二距离传感器400和第四距离传感器500，所述第一距离传感器200与第三距离传感器300之间距离、第二距离传感器400与第四距离传感器500之间的距离均为l，温度传感器600设置于执行末端的中心处，执行末端用于装夹灸头。

本实施例的基于多传感器感知的艾灸机器人运动控制方法具体包括如下步骤：

步骤一：静态建模，控制所述关节机器人100的执行末端在人体表面上方执行任意手法轨迹的运动，同时多个所述距离传感器扫描人体表面，并测量出执行末端与人体体表之间的距离值，机器人控制器根据所述距离传感器采集的距离数值拟合出体表的静态曲面模型并生成执行末端的主动运动轨迹程序，其中关节机器人100的执行末端循环执行手法轨迹运动多次，不断优化曲面模型；

步骤二：根据所述主动运动轨迹程序，机器人控制器控制关节机器人100上的执行末端运动；

步骤三：动态实时补偿，多个距离传感器实时测量执行末端与人体体表的距离值，并计算测量值和设定施灸的高度值之间的误差△eh；温度传感器600实时监测人体体表穴位的温度，并计算测量值和设定的施灸温度值之间的误差△et，如图5所示，机器人控制器采用闭环控制算法拟合出执行末端的随动运动轨迹；

步骤四：机器人控制器把主动运动轨迹和随动运动轨迹采用多项式拟合在一起，最终形成执行末端的叠加运动轨迹，并保证了轨迹的连续和平缓。

进一步地，在步骤一和步骤三中，执行末端与人体体表之间的距离值的具体测量方法如下：第一距离传感器200、第二距离传感器400、第三距离传感器300和第四距离传感器500分别获取到距离数据d1、d2、d3、d4，第一距离传感器200和第三距离传感器300的轴线所组成的平面与关节机器人100b角所处的平面重合，其中第二距离传感器400和第四距离传感器500的轴线所组成的平面与关节机器人100c角所处的平面重合，根据三角函数定理可知，角度计算必须知道三角形中某两边的长度，所以感知人体曲面的法线在机器人的姿态b角平面或者c角的变化角度时必须使用一对距离传感器获取的距离值才能计算出相应的角度。因此需要多个距离传感器才能准确感知计算曲面角度的变化，由于需要保持灸头中心轴线和人体曲面的法线重合，因此需要关节对机器人100的姿态进行控制，使用三角余切公式：tanθ＝(d3-d1)/l可计算出关节机器人100姿态的b角补偿值θ，同理使用d2、d4、l可计算出关节机器人100姿态的c角补偿值α。

并且使用d1、d2、d3、d4、θ和α的值，结合三角函数推导出来的公式得出当前执行末端的实际高度值(即是出执行末端与人体体表之间的距离值)：

h＝(d1+d2+d3+d4)/4)*cos(atan(sqrt(tan(90-θ)*tan(90-θ)+tan(90-α)*tan(90-α)))*π/180)。

在步骤一的静态建模时，多个距离传感器测量并计算得出的多组实际高度值h，机器人控制器将多组的高度值h拟合成曲线，并采用样条曲线插值的方式不断拟合优化，得出人体表面的曲线模型。

并且采用三次样条曲线插值的方式生成执行末端的主动运动轨迹，并对主动运动轨迹进行平滑处理，得出人体体表的静态曲面模型和关节机器人100的主动运动程序。

如图6和图7所示，同时在步骤三中，距离传感器测量计算得出的实际距离值h减去设定的距离值hset，得到距离补偿值△eh，即h-hset＝△eh；关节机器人100控制器实时计算△eh、θ、α，并根据其值进行实时的位置补偿。

在所述执行末端安装有力矩传感器，力矩传感器用于监控执行末端是否与障碍物发生碰撞。

以上对本发明的较佳实施方式进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可作出种种的等同变型或替换，这些等同的变型或替换均包含在本申请权利要求所限定的范围内。