CN116197905B - 一种理疗机器人操作边界位置估计与姿态约束方法 - Google Patents

Abstract

本发明属于理疗机器人领域，其公开了一种理疗机器人操作边界位置估计与姿态约束方法，通过建立坐标系，确定理疗头的移动范围，根据理疗头所处位置及用户移动操作指令，判断理疗头是否可以移动、如何移动，定义机器人操作范围满足理疗应用场景的使用，通过计算理疗头在期望理疗部位的合适位置姿态；优化理疗机器人在该部位工作时的灵活性，进一步提高理疗机器人的可操作性及稳定性。方便用户使用。

Description

一种理疗机器人操作边界位置估计与姿态约束方法

技术领域

本发明涉及理疗机器人领域，特别涉及一种理疗机器人操作边界位置估计与姿态约束方法。

背景技术

目前，在进行理疗疗法时，通常是通过人为重复地进行机械性操作来完成理疗过程，重复的操作占用操作人员大量的时间与精力，造成临床工作效率低。因此，市场上出了了理疗机器人的产品，用于代替操作人员进行重复的理疗作业。

但是，市场上现有的理疗机器人尚未涉及到对理疗工作区域进行限定，无法对执行理疗作业的理疗机器人进行安全操作边界的限定和约束，容易造成机器人的活动超出预期的范围，与其它物体或人发生碰撞、影响理疗工作；也有少数理疗机器人具备活动范围限定的策略，但由于限定策略的目的不同，或是限定策略不佳导致理疗机器人活动范围过小，或是在理疗作业中仍然无法避免与外界物体及人的碰撞。

所以，本方案解决的问题，是如何保证理疗机器人工作时理疗头在规定的移动范围内移动，避免毫无约束地移动从而影响理疗工作、与外界物体或人发生碰撞，进而提供一种简单高效的算法策略解决上述问题，根据理疗头所在的位置进行移动的矢量约束。

发明内容

本发明的目的是提供一种理疗机器人操作边界位置估计与姿态约束方法，实现理疗机器人工作时判断理疗头是否能够继续移动，并保证理疗头在规定的移动范围内移动，避免毫无约束地移动从而影响理疗工作、与外界物体或人发生碰撞。

本发明提供的技术方案为：一种理疗机器人操作边界位置估计方法，包括以下步骤：

S1确定基坐标系，基坐标系以六轴机器人与支撑移动模块连接的一端为原点，记为O点；

S2确定理疗头移动范围，移动范围的边界以基坐标原点为圆心呈圆周分布；

S3识别理疗头所处位置，记为a点；

S31控制矢量记为设移动方向矢量与的夹角绝对值为θ∈[0,π]；设可移动距离为理疗机器人移动设定范围圆的半径，设Oa与aD组成三角形的两边的夹角为

S32判断理疗头是否在移动范围内；

S4用户操作按钮传入方向矢量计算移动距离和理疗头姿态约束。

在上述的理疗机器人操作边界位置估计方法中，当步骤S32中判断理疗头在移动范围内时，步骤S4的计算方法如下：

S41通过用户操作按钮传入方向矢量计算与的夹角θ：

S42求解一元二次方程得到工具坐标系原点和边界范围之间的距离X：

由于理疗头在可移动圆内，即A²<＝B²,恒有解，求得值X>0，根据移动方向单位矢量与移动距离X，得到移动目标位置点D。

在上述的理疗机器人操作边界位置估计方法中，当步骤S32中判断理疗头不在移动范围内时，则计算移动方向矢量与理疗头之间的夹角，并判断是否移动。判断过程如下：计算经过理疗头的切线与Oa之间的夹角ψ：由于且Ψ为正；

若ψ+θ≥π，则移动方向与范围有交点，理疗头可以移动到工作范围内；

若ψ+θ<π，则移动方向与范围无交点，理疗头无法移动到工作范围内，此时限制理疗头的移动。

在上述的理疗机器人操作边界位置估计方法中，当移动方向矢量在理疗头与移动范围边界的两条切线夹角之外，即ψ+θ<π，移动方向与移动范围无交点时，理疗头无法移动到工作范围内，此时限制理疗头的移动。

在上述的理疗机器人操作边界位置估计方法中，当移动方向矢量在理疗头与移动范围边界的两条切线夹角之内或刚好在切线上，即ψ+θ≥π，移动方向与移动范围有交点时，理疗头能够移动，此时步骤S4的计算方法如下：

通过用户操作按钮传入方向矢量计算与的夹角θ；

计算理疗头与移动范围边界的两条切线夹角大小为Ψ：

通过反正弦计算ψ的值范围ψ>0，当与夹角绝对值为θ：

π≤θ+ψ

理疗头可以移动，移动距离为；

计算得到移动距离X。

在上述的理疗机器人操作边界位置估计方法中，当步骤S32中判断理疗头在移动范围边界上时，计算理疗头的移动方向矢量与理疗头的位置矢量夹角，并判断是否移动。

在上述的理疗机器人操作边界位置估计方法中，当理疗头的移动方向矢量与理疗头的位置矢量夹角小于等于90°即cosθ≥0，理疗头无法移动到工作范围内，此时限制理疗头的移动。

在上述的理疗机器人操作边界位置估计方法中，当理疗头的移动方向矢量与理疗头的位置矢量夹角大于90°即cosθ<0时，理疗头能够移动，此时步骤S4的计算方法如下：

S41通过用户操作按钮传入方向矢量计算与的夹角θ；

S42计算在与移动范围圆的交点，并根据余弦定理计算移动距离X。

计算得到移动距离为X,为了满足设备移动操作要求,限定期望点D的位置坐标(x,y)，其中x≥330。

本发明还提供一种上述理疗机器人操作边界位置估计的姿态约束方法，技术方案为：获取理疗机器人的当前位置以及机器人确定理疗部位姿态时的六个关节转角信息，将六个关节转角信息代入理疗头末端位置的笛卡尔坐标，计算得到移动转角q1，再进一步得到确定理疗部位时理疗机器人的末端姿态。

在上述的理疗机器人姿态约束方法中，移动转角q1的计算方法如下：

根据理疗头移动前的位置(x1,y1)，移动后的位置(x2,y2)以及理疗头到基坐标原点的距离A：

理疗机器人的末端姿态计算方法如下：

代入机械臂姿态矩阵：

得到运动到该点时的末端姿态。

本发明在采用上述技术方案后，其具有的有益效果为：

本方案通过设置坐标系、确定理疗头的移动在一定的范围内，保证理疗头正常的工作、避免碰到外物或人；在移动之前，先识别理疗头的位置，并判断是否在移动范围内，从而执行后续的操作，避免了理疗头在移动范围外时“渐行渐远”的情况；同时，理疗头在移动范围边界上或移动范围内时，根据控制矢量及理疗头的位置计算得到移动方向矢量约束，判定何种情况理疗头会超出移动范围因此需限制其移动，何种情况不会超出移动范围从而使其正常移动；此外，本申请还通过关节转角及理疗头的位置计算确定理疗部位时机器人的末端姿态，使机器人进一步实现工作范围的优化。

附图说明

图1是本发明理疗机器人的结构示意图；

图2是本发明理疗头在移动范围内的移动方向示意图；

图3是本发明理疗头不在移动范围内且不可移动的示意图；

图4为本发明理疗头不在移动范围内但可移动的示意图；

图5为本发明理疗头在移动范围边界上的移动方向示意图。

附图标记：1、支撑移动模块；2、六轴机器人；3、理疗头。

具体实施方式

下面结合具体实施方式，对本发明的技术方案作进一步的详细说明，但不构成对本发明的任何限制。

如图1所示，理疗机器人包括支撑移动模块、六轴机器人及理疗头。支撑移动模块起到可移动基座的作用，六轴机器人一端与支撑移动模块模块连接，另一端与理疗头连接。

本申请的理疗机器人操作边界位置估计方法如下：

步骤S1：以六轴机器人与支撑移动模块连接的一端为原点O建立基坐标系；

步骤S2：设置理疗头的移动范围，移动范围的边界以基坐标原点为圆心呈圆周分布；

步骤S3：记理疗头所处的位置为a点；

S31控制矢量记为设移动方向矢量与的夹角绝对值为θ∈[0,π]；设可移动距离为理疗机器人移动设定范围圆的半径，设Oa与aD组成三角形的两边的夹角为

S32判断理疗头是否在移动范围内；

S4用户操作按钮传入方向矢量计算移动距离和理疗头姿态约束。

在上述技术方案中，由于理疗机器人在高度上的移动另有限位策略，本申请在此不做额外的介绍，而将重点放在平面上的移动策略，即本申请的基坐标系在实际作业中为投影到与地面平行的某一水平高度时的平面坐标系。六轴机器人在带动理疗头移动之前，系统先识别理疗头当下所处的位置，判断理疗头是否在预先设定的移动范围内，根据上述判断的结果，再选择不同的计算方法计算移动距离与方向矢量约束，避免理疗头毫无约束地自由移动而影响理疗工作，使得理疗头在预先设定的移动范围内移动，保证其能够正常工作，并且不会触碰到外界的物体或人，提高了安全可靠性。此外，还需要注意的是，在本申请中，为理疗机器人移动设定范围圆的半径，该半径是事先规定好的固定值。

以下根据不同的作业情况分别介绍不同的实施例。

实施例1：

在实际作业中，当步骤S32中判断理疗头在预先设定的移动范围内时，参考图2，步骤S4的计算方法如下：

步骤S41：通过用户操作按钮传入方向矢量计算与的夹角θ：

步骤S42：求解一元二次方程得到工具坐标系原点和边界范围之间的距离X：

由于理疗头在可移动圆内，即A²<＝B²,恒有解，求得值X>0，根据移动方向单位矢量与移动距离X，得到移动目标位置点D。

在上述技术方案中，先识别判断完理疗头当前所处的位置，由于此时理疗头在预先设定的移动范围内，因此对理疗头的约束相对较少，只需保证移动完仍然在移动范围内不超出移动范围的边界即可。因此此时理疗头可以朝任意方向移动，区别在于在不同方向上的移动距离不同。通过余弦定理，可以得到移动方向与的夹角θ和可移动距离X之间的变量关系，根据两者之间的关系便可得到移动方向的矢量约束，从而确保理疗头下一步的移动仍在预先设定的移动范围内，能够进行正常的理疗作业，并且避免碰到外界的物体或人，安全可靠。

实施例2：

在实际作业中，当步骤S32中判断理疗头不在预先设定的移动范围内时，则先计算移动矢量方向与理疗头之间的夹角，再判断是否移动。

在上述技术方案中，理疗头不在预先设定的移动范围内，由于此时存在两种情况，一种情况是移动方向矢量朝向远离设定的移动范围的方向，另一种情况是移动方向矢量朝向靠近设定的移动方向的方向，两种情况需要采用不同的策略，因此先计算移动方向矢量与理疗头之间的夹角，根据夹角再进行后续的判断决定是否要对理疗头的移动进行限制或进行相应的约束。

需要强调的是，由于移动范围边界为一个圆，而此时理疗头的位置为圆外的一个点，因此沿圆周做切线可以得到两条经过理疗头位置的切线，上述技术方案中移动方向矢量与理疗头之间的夹角的判断便是在此基础上进行的。更进一步的，理疗头与移动范围边界的两条切线夹角内外的分辨依据是，两条切线与移动范围边界相交的一侧为夹角之内，反之则为夹角之外。更详尽的判断过程如下：计算经过理疗头的切线与Oa之间的夹角ψ：由于且Ψ为正；

若ψ+θ≥π，则移动方向与范围有交点，理疗头可以移动到工作范围内；

若ψ+θ<π，则移动方向与范围无交点，理疗头无法移动到工作范围内，此时限制理疗头的移动。下文技术方案中类似，不再赘述。

如图3所示，当移动方向矢量在理疗头与移动范围边界的两条切线夹角之外时(需要注意的是，为保持表述上的一致，下文移动矢量仍写作但在图3中画的是下同)，显然，在此种情况下，若不对理疗头的移动进行限制，理疗头将更加远离预设的移动范围，无法移动到工作范围内，因此此时限制理疗头的移动。

如图4所示，当移动方向矢量在理疗头与移动范围边界的夹角之内或刚好在切线上时(需要注意的是，为保持表述上的一致，下文移动矢量仍写作但在图4中画的是下同)，理疗头的移动会靠近移动范围直至到达移动范围内，因此理疗头能够移动，此时步骤S4的计算方法如下：

通过用户操作按钮传入方向矢量计算与的夹角θ：

S42计算理疗头与移动范围边界的两条切线夹角大小为Ψ：

通过反正弦计算ψ的值范围ψ>0，当与夹角绝对值为θ：

π≤θ+ψ

理疗头可以移动，移动距离为；

计算得到移动距离X。

在上述技术方案中，由于当理疗头所处位置在预先设定的移动范围之外时存在两种情况，只有移动方向矢量与移动范围有交点时，理疗头才能顺利地移动到预先设定的移动范围内，因此，在本实施例中，在对理疗头的位置进行识别并判断理疗头在移动范围之外后，还需要继续对移动方向矢量的方向进行进一步的判断，从而分为限制理疗头移动或允许理疗头移动的策略。在确定移动方向朝向移动范围之后，再通过余弦定理根据夹角θ和移动距离X的变量关系确定移动矢量约束。

实施例3：

在实际作业中，如图5所示，当步骤S32中判断理疗头在预先设定的移动范围边界上时(需要注意的是，为保持表述上的一致，下文移动矢量仍写作但在图5中画的是下同)，计算理疗头的移动方向矢量与理疗头的位置矢量夹角，并判断是否移动。

当理疗头的移动方向矢量与理疗头的位置矢量夹角小于等于90°即cosθ≥0，理疗头无法移动到工作范围内，此时限制理疗头的移动。

当理疗头的移动方向矢量与理疗头的位置矢量夹角大于90°即cosθ<0时，理疗头能够移动，此时步骤S4的计算方法如下：

S41通过用户操作按钮传入方向矢量计算与的夹角θ：

S42计算在与移动范围圆的交点，并根据余弦定理计算移动距离X。

计算得到移动距离为X,为了满足设备移动操作要求,限定期望点D的位置坐标(x,y)其中x≥330。

在上述技术方案中，当理疗头恰好在预先设定的移动范围边界上时，此时存在两种情况需要不同的执行策略，因此先计算理疗头的移动方向矢量与理疗头的位置矢量夹角，再进行判断决定是否移动，执行不同的策略。

具体的，如果理疗头的移动方向矢量与理疗头的位置矢量夹角小于等于90°，理疗头会离开移动范围边界，朝向远离移动范围的方向移动，而导致不能在预先设定的移动范围内移动，因此此时需要限制理疗头的移动，避免上述情况发生。而如果理疗头的移动方向矢量与理疗头的位置矢量夹角大于等于90°，此时理疗头可以从移动范围的边界移动到移动范围内，因此能够移动，此时通过余弦定理根据夹角θ和移动距离X的变量关系确定移动矢量约束。

实施例4：

在实际作业中，由于理疗头是在六轴机器人的带动下移动的，因此当理疗头完成确定理疗部位时，机器人的六个关节都会有相对应的转角信息，因此，为了对理疗机器人实现姿态的约束，本实施例在理疗头完成确定理疗部位时获取理疗机器人的当前位置，同时获取该状态下六轴机器人的六个关节转角信息，将六个关节转角信息代入理疗头末端位置的笛卡尔坐标，计算得到移动转角q1，再进一步得到确定理疗部位时理疗机器人的末端姿态。

具体的，移动转角q1的计算方法如下：

首先，获取理疗头移动前的位置(x1,y1)、移动后的位置(x2,y2)，以及理疗头到基坐标原点的距离A，根据上述数据代入以下公式：

根据得到呃呃移动转角，再计算理疗机器人的末端姿态，计算方法如下：

代入机械臂姿态矩阵：

得到运动到该点时的末端姿态。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其它的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (9)

1.一种理疗机器人操作边界位置估计方法，其特征在于，包括以下步骤：

S1确定基坐标系，基坐标系以六轴机器人与支撑移动模块连接的一端为原点，记为0点；

S2确定理疗头移动范围，移动范围的边界以基坐标原点为圆心呈圆周分布；

S3识别理疗头所处位置，记为a点；

S31控制矢量记为设移动方向矢量与的夹角绝对值为θ∈[0，π]；设可移动距离为理疗机器人移动设定范围圆的半径，设Oa与aD组成三角形的两边的夹角为

S32判断理疗头是否在移动范围内；

S4用户操作按钮传入方向矢量计算移动距离和理疗头姿态约束；

当步骤S32中判断理疗头在移动范围内时，步骤S4的计算方法如下：

S41通过用户操作按钮传入方向矢量计算与的夹角θ：

S42求解一元二次方程得到工具坐标系原点和边界范围之间的距离X：

由于理疗头在可移动圆内，即A²<＝B²，恒有解，求得值X>0，根据移动方向单位矢量与移动距离X，得到移动目标位置点D。

2.根据权利要求1所述的理疗机器人操作边界位置估计方法，其特征在于，当步骤S32中判断理疗头不在移动范围内时，则计算移动方向矢量与理疗头之间的夹角，并判断是否移动。

3.根据权利要求2所述的理疗机器人操作边界位置估计方法，其特征在于，当移动方向矢量在理疗头与移动范围边界的两条切线夹角之外时，理疗头无法移动到工作范围内，此时限制理疗头的移动。

4.根据权利要求2所述的理疗机器人操作边界位置估计方法，其特征在于，当移动方向矢量在理疗头与移动范围边界的两条切线夹角之内或刚好在切线上时，理疗头能够移动，此时步骤S4的计算方法如下：

S41通过用户操作按钮传入方向矢量计算与的夹角θ：

S42计算理疗头与移动范围边界的两条切线夹角大小为Ψ

通过反正弦计算ψ的值范围ψ＞0，当与夹角绝对值为θ：

π≤θ+ψ

理疗头可以移动，移动距离为；

计算得到移动距离X。

5.根据权利要求1所述的理疗机器人操作边界位置估计方法，其特征在于，当步骤S32中判断理疗头在移动范围边界上时，计算理疗头的移动方向矢量与理疗头的位置矢量夹角，并判断是否移动。

6.根据权利要求5所述的理疗机器人操作边界位置估计方法，其特征在于，当理疗头的移动方向矢量与理疗头的位置矢量夹角即cosθ≥0，理疗头无法移动到工作范围内，此时限制理疗头的移动。

7.根据权利要求5所述的理疗机器人操作边界位置估计方法，其特征在于，当cosθ<0时，理疗头能够移动，此时步骤S4的计算方法如下：

S41通过用户操作按钮传入方向矢量计算与的夹角θ：

S42计算在与移动范围圆的交点，并根据余弦定理计算移动距离X：

8.根据权利要求1-7任一项所述的理疗机器人操作边界位置估计方法的姿态约束方法，其特征在于，获取理疗机器人的当前位置以及机器人确定理疗部位姿态时的六个关节转角信息，将六个关节转角信息代入理疗头末端位置的旋转矩阵，根据已知的移动期望位置D，计算得到移动转角q1，再进一步代入末端工具坐标系矩阵得到确定理疗部位时理疗机器人的末端姿态。

9.根据权利要求8所述的姿态约束方法，其特征在于，移动转角q1的计算方法如下：

根据理疗头移动前的位置(x1，y1)，移动后的位置(x2，y2)以及理疗头到基坐标原点的距离A：