CN114177008A - 一种基于可变虚拟夹具的上肢康复机器人控制方法及系统 - Google Patents

一种基于可变虚拟夹具的上肢康复机器人控制方法及系统 Download PDF

Info

Publication number
CN114177008A
CN114177008A CN202210030088.6A CN202210030088A CN114177008A CN 114177008 A CN114177008 A CN 114177008A CN 202210030088 A CN202210030088 A CN 202210030088A CN 114177008 A CN114177008 A CN 114177008A
Authority
CN
China
Prior art keywords
constraint
force
virtual
determining
mechanical arm
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210030088.6A
Other languages
English (en)
Other versions
CN114177008B (zh
Inventor
郭帅
方红瑞
宋韬
费思先
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Shanghai for Science and Technology
Original Assignee
University of Shanghai for Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Shanghai for Science and Technology filed Critical University of Shanghai for Science and Technology
Priority to CN202210030088.6A priority Critical patent/CN114177008B/zh
Publication of CN114177008A publication Critical patent/CN114177008A/zh
Application granted granted Critical
Publication of CN114177008B publication Critical patent/CN114177008B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0274—Stretching or bending or torsioning apparatus for exercising for the upper limbs
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/12—Driving means
    • A61H2201/1207—Driving means with electric or magnetic drive
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/16—Physical interface with patient
    • A61H2201/1657—Movement of interface, i.e. force application means
    • A61H2201/1659—Free spatial automatic movement of interface within a working area, e.g. Robot
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50—Control means thereof
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2205/00—Devices for specific parts of the body
    • A61H2205/06—Arms
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • General Physics & Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Computer Hardware Design (AREA)
  • Health & Medical Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • Pure & Applied Mathematics (AREA)
  • Public Health (AREA)
  • Computational Mathematics (AREA)
  • General Health & Medical Sciences (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Veterinary Medicine (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Rehabilitation Therapy (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Pain & Pain Management (AREA)
  • Epidemiology (AREA)
  • Manipulator (AREA)
  • Rehabilitation Tools (AREA)

Abstract

本发明涉及一种基于可变虚拟夹具的上肢康复机器人控制方法及系统。该方法包括根据机械臂的起点位置以及目标位置确定虚拟路径；并根据虚拟路径和末端位置确定最短距离；根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数；根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力；根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力；将当前的引导力和第一约束操作力以及第二约束操作力转化为期望速度，进而根据期望速度控制机械臂执行相应的操作。本发明能够提高虚拟夹具的实时性、精准性和辅助性。

Description

一种基于可变虚拟夹具的上肢康复机器人控制方法及系统
技术领域
本发明涉及机器人辅助医疗康复领域，特别是涉及一种基于可变虚拟夹具的上肢康复机器人控制方法及系统。
背景技术
世界各地由由中风引起的肢体功能障碍患者的人数逐年增加，目前，中风已成为我国成年人群致死致残的首位病因。作为一种急性脑血管疾病，其高发病率、高死亡率和高致残率的特点使70％～80％的患者伴有不同程度的肢体损伤，尤其是上肢功能的丧失会导致日常生活活动困难，严重影响生活质量，因此上肢的康复尤为重要。
机器人作为患者康复过程中的辅助性角色，为上肢偏瘫的患者提供更加可靠的训练。常见的上肢康机器人可以分为末端引导式和外骨骼式两大类。对于末端引导式机器人，由末端牵引患者手臂实现空间内的训练运动。
现有的末端引导式康复机器人虽然能够协助患者延着预设直线实现空间往复式训练。但仍然存在的问题有：(1)对于空间中轨迹训练，不能根据患者手臂在空间中的位置改变相对应的辅助，患者难以收敛到目标点，偏离预设路径距离太大，使训练结果统计中的误差偏大；(2)对于延预定直线的上肢运动，直线路径改变时，对患肢的支撑性较弱，不能提供有效的辅助。
因此，基于现有训练技术中存在的上述问题，需要对传统的虚拟夹具控制方法及系统作出改善，提高训练的效率。
发明内容
本发明的目的是提供一种基于可变虚拟夹具的上肢康复机器人控制方法及系统，能够提高虚拟夹具的实时性、精准性和辅助性。
为实现上述目的，本发明提供了如下方案：
一种基于可变虚拟夹具的上肢康复机器人控制方法，包括：
获取机械臂的目标位置、起点位置以及末端位置；
根据机械臂的起点位置以及目标位置确定虚拟路径；并根据虚拟路径和末端位置确定最短距离；
根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数；
根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力；所述第一约束边界模型用于根据最短路径确定约束策略，并根据约束策略生成约束操作力；
根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力；
将当前的引导力和第一约束操作力以及第二约束操作力转化为期望速度，进而根据期望速度控制机械臂执行相应的操作。
可选地，所述根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数，具体包括：
根据机械臂的末端位置和目标位置确定引导力的方向；
利用公式
Figure BDA0003466021520000021
确定引导力的大小；
其中，FP为引导力，m为助力等级系数，实际代表在该助力等级下引导力的最大值，x为机械臂的末端位置到目标位置之间的距离，r为接近目标位置设定区域的无力区域半径。
可选地，所述根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数，具体包括：
利用公式
Figure BDA0003466021520000022
确定虚拟系数；
其中，k为虚拟系数，k1为刚度系数最小值，k2为刚度系数最大值，Rm为起点位置至目标位置的距离，
Figure BDA0003466021520000023
为由机械臂的末端位置指向目标位置的向量，该向量的正负根据从起点位置指向目标位置的向量来判断，若两个向量的夹角为锐角，则
Figure BDA0003466021520000031
为正，反之，为负。
可选地，所述根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力，具体包括：
利用公式
Figure BDA0003466021520000032
确定第一约束边界模型；
其中，FA为第一约束操作力，D1、D2分别为依次增大的边界阈值，随虚拟夹具的大小进行改变，0到D1为保护边界，D1到D2为延展性边界，大于D2为刚性边界，k为虚拟系数，d为最短距离，nl、nm以及nh分别为保护边界、延展性边界、刚性边界阶段内所采用的二次曲线系数，A为保护边界内所添加的一次项保护力斜率，此后该项力作为一个常数存在。
可选地，所述根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力，具体包括：
利用公式
Figure BDA0003466021520000033
确定第二约束边界模型；
其中，FR为第二约束操作力，dotl为由机械臂的末端位置指向目标位置的向量
Figure BDA0003466021520000034
与从起点位置指向目标位置的向量
Figure BDA0003466021520000035
的向量积，dot2为机械臂的末位置指向起点位置的向量
Figure BDA0003466021520000036
与从目标位置指向起点位置的向量
Figure BDA0003466021520000037
的向量内积，Fmax为在满足dot1＜0或dot2＜0条件下机械臂的末端位置距离起点位置或目标位置的距离为Rend值时所达到的约束力，Rend为期望将机械臂的末端位置超出起点位置或目标位置的距离所限制在的范围。
一种基于可变虚拟夹具的上肢康复机器人控制系统，包括：
位置获取模块，用于获取机械臂的目标位置、起点位置以及末端位置；
虚拟路径和最短距离确定模块，用于根据机械臂的起点位置以及目标位置确定虚拟路径；并根据虚拟路径和末端位置确定最短距离；
引导力和虚拟系数确定模块，用于根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数；
第一约束操作力确定模块，用于根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力；所述第一约束边界模型用于根据最短路径确定约束策略，并根据约束策略生成约束操作力；
第二约束操作力确定模块，用于根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力；
机械臂执行模块，用于将当前的引导力和第一约束操作力以及第二约束操作力转化为期望速度，进而根据期望速度控制机械臂执行相应的操作。
可选地，所述引导力和虚拟系数确定模块具体包括：
引导力的方向确定单元，用于根据机械臂的末端位置和目标位置确定引导力的方向；
引导力的大小确定单元，用于利用公式
Figure BDA0003466021520000041
确定引导力的大小；
其中，FP为引导力，m为助力等级系数，实际代表在该助力等级下引导力的最大值，x为机械臂的末端位置到目标位置之间的距离，r为接近目标位置设定区域的无力区域半径。
可选地，所述引导力和虚拟系数确定模块具体包括：
虚拟系数确定单元，用于利用公式
Figure BDA0003466021520000042
确定虚拟系数；
其中，k为虚拟系数，k1为刚度系数最小值，k2为刚度系数最大值，Rm为起点位置至目标位置的距离，
Figure BDA0003466021520000043
为由机械臂的末端位置指向目标位置的向量，该向量的正负根据从起点位置指向目标位置的向量来判断，若两个向量的夹角为锐角，则
Figure BDA0003466021520000051
为正，反之，为负。
可选地，所述第一约束操作力确定模块具体包括：
第一约束边界模型确定单元，用于利用公式
Figure BDA0003466021520000052
确定第一约束边界模型；
其中，FA为第一约束操作力，D1、D2分别为依次增大的边界阈值，随虚拟夹具的大小进行改变，0到D1为保护边界，D1到D2为延展性边界，大于D2为刚性边界，k为虚拟系数，d为最短距离，nl、nm以及nh分别为保护边界、延展性边界、刚性边界阶段内所采用的二次曲线系数，A为保护边界内所添加的一次项保护力斜率，此后该项力作为一个常数存在。
可选地，所述第二约束操作力确定模块具体包括：
第二约束边界模型确定单元，用于利用公式
Figure BDA0003466021520000053
确定第二约束边界模型；
其中，FR为第二约束操作力，dot1为由机械臂的末端位置指向目标位置的向量
Figure BDA0003466021520000057
与从起点位置指向目标位置的向量
Figure BDA0003466021520000056
的向量积，dot2为机械臂的末位置指向起点位置的向量
Figure BDA0003466021520000054
与从目标位置指向起点位置的向量
Figure BDA0003466021520000055
的向量内积，Fmax为在满足dot1＜0或dot2＜0条件下机械臂的末端位置距离起点位置或目标位置的距离为Rend值时所达到的约束力，Rend为期望将机械臂的末端位置超出起点位置或目标位置的距离所限制在的范围。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明所提供的一种基于可变虚拟夹具的上肢康复机器人控制方法及系统，本发明以预设的虚拟直线路径为参考，考虑了延直线方向上虚拟夹具的收敛作用以帮助患者更精准地到达目标点。以预设的虚拟直线路径为参考，考虑了直线垂直方向上虚拟夹具的切换，在提高对患者手臂支撑度的同时提供一定范围的自由偏离运动。在基于现有虚拟夹具技术引导患者运动、约束运动范围功能的基础上提高了引导的精度，提高了对患者手臂的支撑度，同时又使虚拟夹具的实时切换平滑顺利，使操作始终在安全的环境下进行。基于现有的虚拟夹具技术，在实现引导患者运动、约束超出范围运动的基础上，考虑患者手臂与空间直线训练的起点和终点的相对位置，对虚拟夹具作出改变，使患者能够更精准地到达目标点；通过对机器人末端偏离预设路径的距离进行判断，实时改变虚拟夹具，增加对患者手臂的支撑性。以相对空间直线的纵向及横向分别改变虚拟夹具，提高虚拟夹具的实时性、精准性和辅助性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的一种基于可变虚拟夹具的上肢康复机器人控制方法流程示意图；
图2为本发明优选实施例中虚拟路径上的引导力的示意图；
图3为本发明优选实施例中延虚拟路径直线方向的纵向虚拟系数随操作臂末端相对于起点和终点的位置变化示意图；
图4为本发明优选实施例中虚拟路径两端的约束边界示意图；
图5为本发明优选实施例中虚拟夹具平面示意图；
图6为本发明优选实施例中当虚拟路径改变时，虚拟夹具的切换示意图；
图7为本发明优选实施例中机器人所采用的控制方法的质量-阻尼模型示意图；
图8为本发明所提供的一种基于可变虚拟夹具的上肢康复机器人控制系统结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于可变虚拟夹具的上肢康复机器人控制方法及系统，能够提高虚拟夹具的实时性、精准性和辅助性。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明所提供的一种基于可变虚拟夹具的上肢康复机器人控制方法流程示意图，如图1所示，本发明所提供的一种基于可变虚拟夹具的上肢康复机器人控制方法，包括：
S101，获取机械臂的目标位置、起点位置以及末端位置；获取系统生成的目标位置以及机械臂的末端位置，目标位置由上位机unity界面依据写好的算法生成并发送至下位机，同时发送的还有起点位置。获取t时刻相应的起点位置Ps(Xs(t)，Ys(t)，Zs(t)与目标位置Pe(xe(t)，Ye(t)，Ze(t))；并由机械臂关节角正解出当前得机械臂末端位置Pp(Xp(t)，Yp(t)，Zp(t))；所有点的坐标均为相对于机械臂的基座标系而言。
图6中所示为其中一个平面式训练的界面图，在图中，有一个半径为Rm的圆，圆上有8个点，分别与圆心组成不同的起点和终点(目标位置)。
在目标点被达到以前，起点、目标点不会改变，所述末端位置是UR机械臂根据机械臂关节角正解得到的。并且，系统更换目标点并将当前点作为起点的契机是末端位置到达了目标点附近半径为r的小圆范围以内，此时系统判定目标点与末端位置是属于重合状态。当所有的点均设置成为目标点以及起点之后，一次训练结束，将会开启新一轮的重新训练。
S102，根据机械臂的起点位置以及目标位置确定虚拟路径；并根据虚拟路径和末端位置确定最短距离；在系统判断到达目标位置以前，虚拟路径不会改变。
根据末端位姿和虚拟路径计算最短距离d，如图2中所示，由于训练路径是一条虚拟的直线，因此，最短距离即为Pp与其在直线L上的垂足点Po之间的距离。
如图2以及图6所示，计算虚拟路径的直线方程，更为具体地，该虚拟路径是由起点和终点确定的一条直线
Figure BDA0003466021520000081
在图6所示的训练界面中，该虚拟路径的长度为固定值，等于圆形半径Rm。在三维空间的训练中，需要配合VR眼镜，虚拟路径是空间中的一条直线，长度是变化的。
S103，根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数；
S103具体包括：
根据机械臂的末端位置和目标位置确定引导力的方向
Figure BDA0003466021520000082
将从机器人末端点(末端位置)Pp指向目标点(目标位置)Pe的向量单位化，得到应该提供给患者手臂的引导力方向
Figure BDA0003466021520000083
图2所示的是Pp偏离直线L与在直线L上两种可能出现的情况；
根据Pp是否到达目标点识别区域内，来判断是否施加引导力，引导力的大小按照以下公式变化：
Figure BDA0003466021520000084
其中，FP为引导力，m为助力等级系数，实际代表在该助力等级下引导力的最大值，x为机械臂的末端位置到目标位置之间的距离，r为接近目标位置设定区域的无力区域半径。
式中，m的取值范围为(0，10)N，r的取值为(0，0.01)m；基于本设备的控制方法所定的参数，实际使用中m可根据患者的需求而定，r根据上位机界面中中的参数而定。
关于引导力的施加采用的是按照距离的施加与按照时间的施加相结合；
通过设定无力区域r以及按照距离施加力的方式使得接近目标点的时候，机械臂末端速度降为较小值；
更进一步地，如图6中所示，以Ps作为起点，圆心为终点Pe，当到达终点时，虚拟路径所在的直线方程由L切换为L’，随着下一个目标点Pe′的生成，引导力由FA变为FA′，则产生了突变，力的变化不连续，在目标点附近设定的无力区域半径r可以对这种突变起到一定的缓冲作用，但仅采用这种距离施加力的方式要使得FA减为0、FA′增大为指定值，对r要求较大，r的增大会削弱引导力以及目标点的精度，因此还要按照时间变化作为辅助；
更进一步地，按照时间施加力的具体方法为，捕捉到目标点的变换后，系统自动将FA减小，将FA′增大，两个操作同时减小，并且按照时间，进程每循环一次执行一次减小和增大操作，以连续的变化完成引导力的切换。
S103具体包括：
利用公式
Figure BDA0003466021520000091
确定虚拟系数；
其中，k为虚拟系数，k1为刚度系数最小值，k2为刚度系数最大值，Rm为起点位置至目标位置的距离，
Figure BDA0003466021520000092
为由机械臂的末端位置指向目标位置的向量，该向量的正负根据从起点位置指向目标位置的向量来判断，若两个向量的夹角为锐角，则
Figure BDA0003466021520000093
为正，反之，为负。
如图3所示，为k随
Figure BDA0003466021520000094
的变化示意图，纵向指的是延着直线L的方向，具体分析，k的变化分两段，RII区域代表的实际上是Po位于Ps和Pe之间的时候，RI区域则是Po位于直线L上其它地方。
更进一步地，Po位于Ps端时，k较小，末端位置到虚拟路径的最短距离d有一定的变化空间，随着Po靠近Pe，k变大，直到达到最大值k2，d的变化空间受到了约束，体现了虚拟夹具纵向变化带来的收敛效果，提高了到达目标点的精准度。
S104，根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力；所述第一约束边界模型用于根据最短路径确定约束策略，并根据约束策略生成约束操作力；根据机械臂末端点Pp至其在直线L上的垂足点Po的距离d的大小，将约束边界模型分为三个部分；
S104具体包括：
利用公式
Figure BDA0003466021520000101
确定第一约束边界模型；
其中，FA为第一约束操作力，D1、D2分别为依次增大的边界阈值，随虚拟夹具的大小进行改变，0到D1为保护边界，D1到D2为延展性边界，大于D2为刚性边界，k为虚拟系数，d为最短距离，nl、nm以及nh分别为保护边界、延展性边界、刚性边界阶段内所采用的二次曲线系数，A为保护边界内所添加的一次项保护力斜率，此后该项力作为一个常数存在。
如图5中所示，当d小于预设阈值D1时采用保护性边界，此时的约束力由二次项和一次项两部分组成，其中，二次项的系数nl取较小值；二次函数使边界的切换趋于平滑，为了弥补二次函数起始增长缓慢、提供保护力不足的缺陷，在力较小范围D1内提供斜率为A的一次项保护力；
当d大于预设阈值D1且小于预设阈值D2时采用延展性边界，约束力增长较慢，最短距离d仍然可以增长至较大值；此时D1阶段的力作为一个常数值存在于D1至D2阶段，二次项的系数增长为nm；
当d大于预设阈值D2时采用刚性边界，此时约束力已经达到较大值，为防止最短距离d的继续增长，nh取较大值，约束力的增长进入快速阶段，直至机械臂达到保护性停止。
利用公式
Figure BDA0003466021520000102
确定nl，nm以及nk；
S105，根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力；
根据机械臂末端点与起点、终点的相对位置生成位于虚拟路径两端的约束边界模型，如图3和图4所示，虚拟路径两端是指图3中的RI区域，对延直线L方向的运动进行约束；图4中仅画出了Pe端的约束边界，Ps端的情况相同；
当Po从RII区域切换到RI区域时，约束力的方向由Pp指向Po变为由Pp指向Pe或Ps；
利用公式
Figure BDA0003466021520000111
确定第二约束边界模型；
其中，FR为第二约束操作力，dot1为由机械臂的末端位置指向目标位置的向量
Figure BDA0003466021520000112
与从起点位置指向目标位置的向量
Figure BDA0003466021520000113
的向量积，dot2为机械臂的末位置指向起点位置的向量
Figure BDA0003466021520000114
与从目标位置指向起点位置的向量
Figure BDA0003466021520000115
的向量内积，Fmax为在满足dot1＜0或dot2＜0条件下机械臂的末端位置距离起点位置或目标位置的距离为Rend值时所达到的约束力，Rend为期望将机械臂的末端位置超出起点位置或目标位置的距离所限制在的范围。
图5所示为患者在平面训练中的虚拟夹具示意图，延直线L方向分为R I和RII两个区域，所对应的虚拟系数k不同，实现虚拟夹具纵向可变；在垂直于L方向分为D1、D2、Dmax三个梯度，每个梯度对应不同的边界力，根据最短距离d的不同，虚拟夹具横向可变。
图6所示为当虚拟路径改变时，虚拟夹具的切换，对于引导力FA，采用的是按照距离施加力与按照时间施加力的结合；而对于约束反力FP和FR由于不会直接影响到机械臂末端的速度，因此只采用按照时间施加的方式。
S106，将当前的引导力和第一约束操作力以及第二约束操作力转化为期望速度，进而根据期望速度控制机械臂执行相应的操作。
机械臂采用的控制方式是导纳控制，机械臂末端的六维力传感器测得患者手臂的输入力作为主动力，由系统计算出被动力，将主动力和被动力分别进行处理之后输入图7所示的控制系统中，计算除末端的期望速度，计算模型如下：
Figure BDA0003466021520000121
式中，M为惯性特性，D为阻尼特性，K为刚度特性，fe为工具坐标系下传感器所测得的力与力矩，Δx＝x0-xd，其中，xd，
Figure BDA0003466021520000122
为机器人的期望位姿、速度和加速度，而x0，
Figure BDA0003466021520000123
为机器人在外力为零时理论上需要跟踪的位置以及速度、加速度值。KΔx为计算机器人的期望位置时所需要考虑的弹性力，其中，弹性力可分为引导力FA、约束反力FP和FR三部分。
图8为本发明所提供的一种基于可变虚拟夹具的上肢康复机器人控制系统结构示意图，如图8所示，本发明所提供的一种基于可变虚拟夹具的上肢康复机器人控制系统，包括：
位置获取模块801，用于获取机械臂的目标位置、起点位置以及末端位置；
虚拟路径和最短距离确定模块802，用于根据机械臂的起点位置以及目标位置确定虚拟路径；并根据虚拟路径和末端位置确定最短距离；
引导力和虚拟系数确定模块803，用于根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数；
第一约束操作力确定模块804，用于根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力；所述第一约束边界模型用于根据最短路径确定约束策略，并根据约束策略生成约束操作力；
第二约束操作力确定模块805，用于根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力；
机械臂执行模块806，用于将当前的引导力和第一约束操作力以及第二约束操作力转化为期望速度，进而根据期望速度控制机械臂执行相应的操作。
所述引导力和虚拟系数确定模块803具体包括：
引导力的方向确定单元，用于根据机械臂的末端位置和目标位置确定引导力的方向；
引导力的大小确定单元，用于利用公式
Figure BDA0003466021520000124
确定引导力的大小；
其中，FP为引导力，m为助力等级系数，实际代表在该助力等级下引导力的最大值，x为机械臂的末端位置到目标位置之间的距离，r为接近目标位置设定区域的无力区域半径。
所述引导力和虚拟系数确定模块803具体包括：
虚拟系数确定单元，用于利用公式
Figure BDA0003466021520000131
确定虚拟系数；
其中，k为虚拟系数，k1为刚度系数最小值，k2为刚度系数最大值，Rm为起点位置至目标位置的距离，
Figure BDA0003466021520000132
为由机械臂的末端位置指向目标位置的向量，该向量的正负根据从起点位置指向目标位置的向量来判断，若两个向量的夹角为锐角，则
Figure BDA0003466021520000133
为正，反之，为负。
所述第一约束操作力确定模块804具体包括：
第一约束边界模型确定单元，用于利用公式
Figure BDA0003466021520000134
确定第一约束边界模型；
其中，FA为第一约束操作力，D1、D2分别为依次增大的边界阈值，随虚拟夹具的大小进行改变，0到D1为保护边界，D1到D2为延展性边界，大于D2为刚性边界，k为虚拟系数，d为最短距离，nl、nm以及nh分别为保护边界、延展性边界、刚性边界阶段内所采用的二次曲线系数，A为保护边界内所添加的一次项保护力斜率，此后该项力作为一个常数存在。
所述第二约束操作力确定模块805具体包括：
第二约束边界模型确定单元，用于利用公式
Figure BDA0003466021520000141
确定第二约束边界模型；
其中，FR为第二约束操作力，dot1为由机械臂的末端位置指向目标位置的向量
Figure BDA0003466021520000142
与从起点位置指向目标位置的向量
Figure BDA0003466021520000143
的向量积，dot2为机械臂的末位置指向起点位置的向量
Figure BDA0003466021520000144
与从目标位置指向起点位置的向量
Figure BDA0003466021520000145
的向量内积，Fmax为在满足dot1＜0或dot2＜0条件下机械臂的末端位置距离起点位置或目标位置的距离为Rend值时所达到的约束力，Rend为期望将机械臂的末端位置超出起点位置或目标位置的距离所限制在的范围。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种基于可变虚拟夹具的上肢康复机器人控制方法，其特征在于，包括：
获取机械臂的目标位置、起点位置以及末端位置；
根据机械臂的起点位置以及目标位置确定虚拟路径；并根据虚拟路径和末端位置确定最短距离；
根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数；
根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力；所述第一约束边界模型用于根据最短路径确定约束策略，并根据约束策略生成约束操作力；
根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力；
将当前的引导力和第一约束操作力以及第二约束操作力转化为期望速度，进而根据期望速度控制机械臂执行相应的操作。
2.根据权利要求1所述的一种基于可变虚拟夹具的上肢康复机器人控制方法，其特征在于，所述根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数，具体包括：
根据机械臂的末端位置和目标位置确定引导力的方向；
利用公式
Figure FDA0003466021510000011
确定引导力的大小；
其中，FP为引导力，m为助力等级系数，实际代表在该助力等级下引导力的最大值，x为机械臂的末端位置到目标位置之间的距离，r为接近目标位置设定区域的无力区域半径。
3.根据权利要求2所述的一种基于可变虚拟夹具的上肢康复机器人控制方法，其特征在于，所述根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数，具体包括：
利用公式
Figure FDA0003466021510000021
确定虚拟系数；
其中，k为虚拟系数，k1为刚度系数最小值，k2为刚度系数最大值，Rm为起点位置至目标位置的距离，
Figure FDA0003466021510000022
为由机械臂的末端位置指向目标位置的向量，该向量的正负根据从起点位置指向目标位置的向量来判断，若两个向量的夹角为锐角，则
Figure FDA0003466021510000023
为正，反之，为负。
4.根据权利要求1所述的一种基于可变虚拟夹具的上肢康复机器人控制方法，其特征在于，所述根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力，具体包括：
利用公式
Figure FDA0003466021510000024
确定第一约束边界模型；
其中，FA为第一约束操作力，D1、D2分别为依次增大的边界阈值，随虚拟夹具的大小进行改变，0到D1为保护边界，D1到D2为延展性边界，大于D2为刚性边界，k为虚拟系数，d为最短距离，nl、nm以及nh分别为保护边界、延展性边界、刚性边界阶段内所采用的二次曲线系数，A为保护边界内所添加的一次项保护力斜率，此后该项力作为一个常数存在。
5.根据权利要求1所述的一种基于可变虚拟夹具的上肢康复机器人控制方法，其特征在于，所述根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力，具体包括：
利用公式
Figure FDA0003466021510000025
确定第二约束边界模型；
其中，FR为第二约束操作力，dot1为由机械臂的末端位置指向目标位置的向量
Figure FDA0003466021510000031
与从起点位置指向目标位置的向量
Figure FDA0003466021510000032
的向量积，dot2为机械臂的末位置指向起点位置的向量
Figure FDA0003466021510000033
与从目标位置指向起点位置的向量
Figure FDA0003466021510000034
的向量内积，Fmax为在满足dot1＜0或dot2＜0条件下机械臂的末端位置距离起点位置或目标位置的距离为Rend值时所达到的约束力，Rend为期望将机械臂的末端位置超出起点位置或目标位置的距离所限制在的范围。
6.一种基于可变虚拟夹具的上肢康复机器人控制系统，其特征在于，包括：
位置获取模块，用于获取机械臂的目标位置、起点位置以及末端位置；
虚拟路径和最短距离确定模块，用于根据机械臂的起点位置以及目标位置确定虚拟路径；并根据虚拟路径和末端位置确定最短距离；
引导力和虚拟系数确定模块，用于根据机械臂的末端位置和目标位置确定作用在机械臂末端的引导力和虚拟系数；
第一约束操作力确定模块，用于根据虚拟系数以及最短路径以及第一约束边界模型确定第一约束操作力；所述第一约束边界模型用于根据最短路径确定约束策略，并根据约束策略生成约束操作力；
第二约束操作力确定模块，用于根据机械臂的末端位置与起点位置、目标位置的相对位置构建位于虚拟路径两端的第二约束边界模型，并利用第二约束边界模型确定第二约束操作力；
机械臂执行模块，用于将当前的引导力和第一约束操作力以及第二约束操作力转化为期望速度，进而根据期望速度控制机械臂执行相应的操作。
7.根据权利要求6所述的一种基于可变虚拟夹具的上肢康复机器人控制系统，其特征在于，所述引导力和虚拟系数确定模块具体包括：
引导力的方向确定单元，用于根据机械臂的末端位置和目标位置确定引导力的方向；
引导力的大小确定单元，用于利用公式
Figure FDA0003466021510000035
确定引导力的大小；
其中，FP为引导力，m为助力等级系数，实际代表在该助力等级下引导力的最大值，x为机械臂的末端位置到目标位置之间的距离，r为接近目标位置设定区域的无力区域半径。
8.根据权利要求7所述的一种基于可变虚拟夹具的上肢康复机器人控制系统，其特征在于，所述引导力和虚拟系数确定模块具体包括：
虚拟系数确定单元，用于利用公式
Figure FDA0003466021510000041
确定虚拟系数；
其中，k为虚拟系数，k1为刚度系数最小值，k2为刚度系数最大值，Rm为起点位置至目标位置的距离，
Figure FDA0003466021510000042
为由机械臂的末端位置指向目标位置的向量，该向量的正负根据从起点位置指向目标位置的向量来判断，若两个向量的夹角为锐角，则
Figure FDA0003466021510000043
为正，反之，为负。
9.根据权利要求6所述的一种基于可变虚拟夹具的上肢康复机器人控制系统，其特征在于，所述第一约束操作力确定模块具体包括：
第一约束边界模型确定单元，用于利用公式
Figure FDA0003466021510000044
确定第一约束边界模型；
其中，FA为第一约束操作力，D1、D2分别为依次增大的边界阈值，随虚拟夹具的大小进行改变，0到D1为保护边界，D1到D2为延展性边界，大于D2为刚性边界，k为虚拟系数，d为最短距离，nl、nm以及nh分别为保护边界、延展性边界、刚性边界阶段内所采用的二次曲线系数，A为保护边界内所添加的一次项保护力斜率，此后该项力作为一个常数存在。
10.根据权利要求6所述的一种基于可变虚拟夹具的上肢康复机器人控制系统，其特征在于，所述第二约束操作力确定模块具体包括：
第二约束边界模型确定单元，用于利用公式
Figure FDA0003466021510000051
确定第二约束边界模型；
其中，FR为第二约束操作力，dot1为由机械臂的末端位置指向目标位置的向量
Figure FDA0003466021510000052
与从起点位置指向目标位置的向量
Figure FDA0003466021510000053
的向量积，dot2为机械臂的末位置指向起点位置的向量
Figure FDA0003466021510000054
与从目标位置指向起点位置的向量
Figure FDA0003466021510000055
的向量内积，Fmax为在满足dot1＜0或dot2＜0条件下机械臂的末端位置距离起点位置或目标位置的距离为Rend值时所达到的约束力，Rend为期望将机械臂的末端位置超出起点位置或目标位置的距离所限制在的范围。
CN202210030088.6A 2022-01-12 2022-01-12 一种基于可变虚拟夹具的上肢康复机器人控制方法及系统 Active CN114177008B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210030088.6A CN114177008B (zh) 2022-01-12 2022-01-12 一种基于可变虚拟夹具的上肢康复机器人控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210030088.6A CN114177008B (zh) 2022-01-12 2022-01-12 一种基于可变虚拟夹具的上肢康复机器人控制方法及系统

Publications (2)

Publication Number Publication Date
CN114177008A true CN114177008A (zh) 2022-03-15
CN114177008B CN114177008B (zh) 2022-12-02

Family

ID=80606799

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210030088.6A Active CN114177008B (zh) 2022-01-12 2022-01-12 一种基于可变虚拟夹具的上肢康复机器人控制方法及系统

Country Status (1)

Country Link
CN (1) CN114177008B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2004095520A2 (en) * 2003-04-22 2004-11-04 Berkeley Process Control, Inc. System of path planning for robotic manipulators based on maximum acceleration and finite jerk constraints
CN109171965A (zh) * 2018-09-27 2019-01-11 上海电气集团股份有限公司 基于虚拟夹具的机器人的控制方法及系统
US20190070731A1 (en) * 2017-09-06 2019-03-07 Stryker Corporation Techniques For Controlling Position Of An End Effector Of A Robotic Device Relative To A Virtual Constraint
CN109568082A (zh) * 2018-12-11 2019-04-05 上海大学 一种上肢康复训练机器人及上肢康复训练方法
CN110497403A (zh) * 2019-08-05 2019-11-26 上海大学 一种改进双向rrt算法的机械臂运动规划方法
CN110977974A (zh) * 2019-12-11 2020-04-10 遨博（北京）智能科技有限公司 一种机器人规避奇异位型的导纳控制方法、装置及系统
CN111660307A (zh) * 2020-05-27 2020-09-15 华中科技大学 一种机器人操作高辅精度的虚拟夹具控制方法及系统
CN113814985A (zh) * 2021-10-29 2021-12-21 遨博（北京）智能科技有限公司 一种机器人控制方法、控制柜及系统

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2004095520A2 (en) * 2003-04-22 2004-11-04 Berkeley Process Control, Inc. System of path planning for robotic manipulators based on maximum acceleration and finite jerk constraints
US20190070731A1 (en) * 2017-09-06 2019-03-07 Stryker Corporation Techniques For Controlling Position Of An End Effector Of A Robotic Device Relative To A Virtual Constraint
CN109171965A (zh) * 2018-09-27 2019-01-11 上海电气集团股份有限公司 基于虚拟夹具的机器人的控制方法及系统
CN109568082A (zh) * 2018-12-11 2019-04-05 上海大学 一种上肢康复训练机器人及上肢康复训练方法
CN110497403A (zh) * 2019-08-05 2019-11-26 上海大学 一种改进双向rrt算法的机械臂运动规划方法
CN110977974A (zh) * 2019-12-11 2020-04-10 遨博（北京）智能科技有限公司 一种机器人规避奇异位型的导纳控制方法、装置及系统
CN111660307A (zh) * 2020-05-27 2020-09-15 华中科技大学 一种机器人操作高辅精度的虚拟夹具控制方法及系统
CN113814985A (zh) * 2021-10-29 2021-12-21 遨博（北京）智能科技有限公司 一种机器人控制方法、控制柜及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
尹培坤,王葳,郭帅: "基于笛卡尔阻抗控制的人机协作机械臂性能约束", 《工业控制计算机》 *
黄松恩等: "基于二次映射方法上肢康复训练的虚拟路径研究", 《中国康复医学杂志》 *

Also Published As

Publication number Publication date
CN114177008B (zh) 2022-12-02

Similar Documents

Publication Publication Date Title
Koenemann et al. Real-time imitation of human whole-body motions by humanoids
US20220019196A1 (en) Feedforward control method for floating base dynamics, computer-readable storage medium and robot
JP5330138B2 (ja) 強化学習システム
CN111660307A (zh) 一种机器人操作高辅精度的虚拟夹具控制方法及系统
KR20130005586A (ko) 매니퓰레이터 및 그 경로 생성 방법
Su et al. Robust grasping for an under-actuated anthropomorphic hand under object position uncertainty
JP2012071358A (ja) ロボットおよびその行動制御システム
Yamakawa et al. Dynamic manipulation of a cloth by high-speed robot system using high-speed visual feedback
CN108897220A (zh) 一种自适应稳定平衡控制方法和系统以及双足仿人机器人
CN111515938B (zh) 一种继承型迭代学习控制的下肢外骨骼行走轨迹跟踪方法
CN112237524A (zh) 柔性上肢康复机器人被动康复训练的控制方法
CN113568422B (zh) 基于模型预测控制优化强化学习的四足机器人控制方法
CN115502979A (zh) 一种机械臂力矩主动柔顺精确控制方法及系统
CN114177008B (zh) 一种基于可变虚拟夹具的上肢康复机器人控制方法及系统
CN108098760A (zh) 一种新的双足机器人行走控制装置及方法
CN114851171B (zh) 下肢外骨骼康复机器人步态轨迹跟踪控制方法
CN112476429B (zh) 一种机器人步态控制方法
JP3937108B2 (ja) ロボットの制御方法およびロボット
Faraj et al. Modeling and robust computed torque control for lower limb exoskeleton contacting with ground
Orbik et al. Human hand motion retargeting for dexterous robotic hand
Gu et al. Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following
Zhang et al. Gait Prediction and Assist Control of Lower Limb Exoskeleton Based on Inertia Measurement Unit
Ferreira et al. SVR controller for a biped robot in the sagittal plane with human-based ZMP trajectory reference and gait
Mancisidor et al. Enhanced force control using force estimation and nonlinearity compensation for the Universal Haptic Pantograph
Li et al. Design of Tai-Chi Push-Hands Robot Control System and Construction of Visual Platform

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant