一种基于RCM机器人运动的避障控制方法技术

本发明专利技术涉及基于RCM机器人运动的避障控制方法,包括:具有第一操作区域以及位于第一操作区域内的第二操作区域，执行机构从第一操作区域外表面上的点A以进入角度θ1并沿第一进入直线预进入第一操作区域；判断执行机构的第一进入直线是否与第二操作区域相交，如相交，执行结构沿第一进入直线方向入针深度为L1，随后绕远程运动中心点A进行RCM旋转θ2得到进针角度所在的第二进入直线；否则执行结构沿第一进入直线方向入针深度为L2，随后绕远程运动中心点A进行RCM旋转θ3得到进针角度所在的第二进入直线；执行机构沿第二进入直线进针深度为L3至执行深度；执行机构绕远程运动中心点A进行RCM旋转θ7至注射角度。本发明专利技术的技术方案能自主规避第二操作区域。自主规避第二操作区域。自主规避第二操作区域。

【技术实现步骤摘要】
一种基于RCM机器人运动的避障控制方法


[0001]本专利技术涉及自动化控制领域，更具体地，涉及一种基于
RCM
机器人运动的避障控制方法
。

技术介绍

[0002]RCM(Remote Center of Motion)
远心点约束是一种以远心点为约束定点，在约束定点无物理旋转关节但可绕远心点旋转的一种机构约束方式
。RCM
机器人可以在不破坏结构表面的前提下，在结构内部执行操作
。
在执行操作的过程中，
RCM
充当执行机构的夹持手臂，可绕结构表面的开口自由转动和进出且不与开口发生碰撞
。
[0003]现有的
RCM
机器人主要包括球形机械臂
、
平行四边形机构和圆弧机构
。
对于在结构内部进行操作的需求只提供了结构上的方案
。
实际操作中，为了解决多界面和避障问题，需要在
RCM
机器人结构的基础上提供其执行机构运动路径的算法
。
[0004]专利
CN114452003A
公开了一种手术机器人控制方法，包括：采集主手处操作者的手部动作速度；根据所述手部动作速度控制从手的运动：根据所述手部动作速度控制从手的运动：根据所述手部动作速度控制从手的运动：先控制通用机器人带动器械机器人穿过目标位置进入目标物，再联动控制所述通用机器人绕远心点运动和所述器械机器人伸缩运动，使得所述器械机器人进行设定操作
。r/>上述手术机器人控制方法通过主从手的控制方式控制手术机器人，控制精度高，操作便捷精准；在从手的控制方式中采用通用机器人和器械机器人分工的方式，既可适用于绕远心点实现伸缩旋转
、
又可适用于调整远心点位置
。
[0005]现有的技术方案能够使
RCM
机器人在进入结构表面后，在结构内部绕远心点进行高精度旋转和伸缩
。
但是，仍然不能实现在结构内部自主避障的功能
。

技术实现思路

[0006]本专利技术为克服上述现有技术不能够自主避障，导致在结构内部进行操作时易对内部结构造成破坏的问题，提供一种基于
RCM
机器人运动的避障控制方法
。
[0007]为解决上述技术问题，本专利技术采用的技术方案是：一种基于
RCM
机器人运动的避障控制方法
,
包括以下步骤
:
[0008]S1
：具有第一操作区域以及位于所述第一操作区域内的第二操作区域，执行机构从所述第一操作区域外表面上的点
A
以进入角度
θ1并沿第一进入直线预进入所述第一操作区域；
[0009]S2
：判断所述执行机构的第一进入直线是否与所述第二操作区域相交，如相交，则执行步骤
S3
；如不相交，则执行步骤
S4
；
[0010]S3
：所述执行结构沿第一进入直线方向入针深度为
L1，随后绕远程运动中心点
A
进行
RCM
旋转
θ2得到进针角度所在的第二进入直线，然后再执行步骤
S5
；
[0011]S4
：所述执行结构沿第一进入直线方向入针深度为
L2，随后绕远程运动中心点
A
进行
RCM
旋转
θ3得到进针角度所在的第二进入直线，然后再执行步骤
S5
；
[0012]S5
：所述执行机构沿第二进入直线进针深度为
L3至执行深度；
[0013]S6
：所述执行机构绕远程运动中心点
A
进行
RCM
旋转
θ7至注射角度
。
[0014]优选地，在所述步骤
S1
中，所述第一操作区域为外径为
r2，内径为
r2‑
d
的圆环；所述第二操作区域为与所述圆环同心的圆，所述第二操作区域半径为
r1，且
r1<r2‑
d。
[0015]优选地，在所述步骤
S2
中，通过以下步骤判断所述第一进入直线是否与所述第二操作区域相交：
[0016]S21
：计算
A
点坐标
(x
A
,y
A
)
：其中，
h
为点
A
到赤道径的距离；
[0017]S22
：通过公式：
[0018][0019]计算过点
A
且与所述第二操作区域边界相切的直线角度的绝对值，其中，
K1为所述第二操作区域边界过点
A
的切线中斜率最小的一条切线的斜率，切点为
E
，
θ6为切线角度的绝对值；
[0020]S23
：若进入角度
θ1小于
θ6且大于
45
°
，则所述执行机构的所述第一进入直线会与所述第二操作区域相交，若进入角度
θ1大于
θ6，则所述执行机构的所述第一进入直线不与所述第二操作区域相交
。
[0021]优选地，第一进入直线与第一操作区域内表面的交点为
I
，点
I
的坐标为
(x
I
,y
I
)
；
[0022]由左式得点
I
坐标
(x
I
,y
I
)。
[0023]优选地，在所述步骤
S4
中，若所述第一进入直线不与第二操作区域相交，所述执行机构入针深度
L2应满足穿过第一操作区域边界即大于线段
AI
的长度，即
[0024]由左式得点
I
坐标
(x
I
,y
I
)
[0025][0026]在所述步骤
S3
中，若所述第一进入直线与第二操作区域相交，第一进入直线与第二操作区域的交点为
C
，点
C
的坐标为
(x
c
,y
c
)
；所述执行机构入针深度
L1应满足穿过第一操作区域边界即大于线段
AI
的长度且小于线段
AC
的长度；
[0027]由左式得点
C
坐标
(x
C
,y
C
)
[0028][0029]优选地，在所述步骤
S4
中，若所述第一进入直线不与第二操作区域相交，入针深度
L2应满足下式：
[003本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种基于
RCM
机器人运动的避障控制方法
,
其特征在于
,
包括以下步骤
:S1
：具有第一操作区域以及位于所述第一操作区域内的第二操作区域，执行机构从所述第一操作区域外表面上的点
A
以进入角度
θ1并沿第一进入直线预进入所述第一操作区域；
S2
：判断所述执行机构的第一进入直线是否与所述第二操作区域相交，如相交，则执行步骤
S3
；如不相交，则执行步骤
S4
；
S3
：所述执行结构沿第一进入直线方向入针深度为
L1，随后绕远程运动中心点
A
进行
RCM
旋转
θ2得到进针角度所在的第二进入直线，然后再执行步骤
S5
；
S4
：所述执行结构沿第一进入直线方向入针深度为
L2，随后绕远程运动中心点
A
进行
RCM
旋转
θ3得到进针角度所在的第二进入直线，然后再执行步骤
S5
；
S5
：所述执行机构沿第二进入直线进针深度为
L3至执行深度；
S6
：所述执行机构绕远程运动中心点
A
进行
RCM
旋转
θ7至注射角度
。2.
根据权利要求1所述的基于
RCM
机器人运动的避障控制方法，其特征在于，在所述步骤
S1
中，所述第一操作区域为外径为
r2，内径为
r2‑
d
的圆环；所述第二操作区域为与所述圆环同心的圆，所述第二操作区域半径为
r1，且
r1<r2‑
d。3.
根据权利要求2所述的基于
RCM
机器人运动的避障控制方法
,
其特征在于
,
在所述步骤
S2
中，通过以下步骤判断所述第一进入直线是否与所述第二操作区域相交：
S21
：计算
A
点坐标其中，
h
为点
A
到赤道径的距离；
S22
：通过公式：计算过点
A
且与所述第二操作区域边界相切的直线角度的绝对值，其中，
K1为所述第二操作区域边界过点
A
的切线中斜率最小的一条切线的斜率，切点为
E
，
θ6为切线角度的绝对值；
S23
：若进入角度
θ1小于
θ6且大于
45
°
，则所述执行机构的所述第一进入直线会与所述第二操作区域相交，若进入角度
θ1大于
θ6，则所述执行机构的所述第一进入直线不与所述第二操作区域相交
。4.
根据权利要求3所述的基于
RCM
机器人运动的避障控制方法，其特征在于，第一进入直线与第一操作区域内表面的交点为
I
，点
I
的坐标为
(x
i
,y
i
)
；由左式得点
I
坐标
(x
i
,y
I
)。5.
根据权利要求4所述的基于
RCM
机器人运动的避障控制方法，其特征在于，在所述步骤
S4
中，若所述第一进入直线不与第二操作区域相交，所述执行机构入针深度
L2应满足穿过第一操作区域边界即大于线段
AI
的长度，即
由左式得点
I
坐标
(x
I
,y
I
)
在所述步骤
S3
中，若所述第一进入直线与第二操作区域相交，第一进入直线与第二操作区域的交点为
C
，点
C
的坐标为
(x
c
,y
c
)
；所述执行机构入针深度
L1应满足穿过第一操作区域边界即大于线段
AI
的长度且小于线段
AC
的长度；由左式得点
C
坐标
(x
C
,y
C
)6.
根据权利要求5所述的基于...

【专利技术属性】
技术研发人员：晏丕松，陈若曦，林生智，
申请(专利权)人：广州市微眸医疗器械有限公司，
类型：发明
国别省市：