CN117959146A - 一种六自由度的上肢康复机器人控制方法及系统 - Google Patents

一种六自由度的上肢康复机器人控制方法及系统 Download PDF

Info

Publication number
CN117959146A
CN117959146A CN202410390169.6A CN202410390169A CN117959146A CN 117959146 A CN117959146 A CN 117959146A CN 202410390169 A CN202410390169 A CN 202410390169A CN 117959146 A CN117959146 A CN 117959146A
Authority
CN
China
Prior art keywords
motor
motors
upper limb
coordinate system
limb rehabilitation
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
CN202410390169.6A
Other languages
English (en)
Other versions
CN117959146B (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.)
Jiangxi Qiushi Higher Research Institute
Original Assignee
Jiangxi Qiushi Higher Research Institute
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 Jiangxi Qiushi Higher Research Institute filed Critical Jiangxi Qiushi Higher Research Institute
Priority to CN202410390169.6A priority Critical patent/CN117959146B/zh
Publication of CN117959146A publication Critical patent/CN117959146A/zh
Application granted granted Critical
Publication of CN117959146B publication Critical patent/CN117959146B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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
    • A—HUMAN NECESSITIES
    • A63—SPORTS; GAMES; AMUSEMENTS
    • A63B—APPARATUS FOR PHYSICAL TRAINING, GYMNASTICS, SWIMMING, CLIMBING, OR FENCING; BALL GAMES; TRAINING EQUIPMENT
    • A63B23/00—Exercising apparatus specially adapted for particular parts of the body
    • A63B23/035—Exercising apparatus specially adapted for particular parts of the body for limbs, i.e. upper or lower limbs, e.g. simultaneously
    • A63B23/12—Exercising apparatus specially adapted for particular parts of the body for limbs, i.e. upper or lower limbs, e.g. simultaneously for upper limbs or related muscles, e.g. chest, upper back or shoulder muscles
    • 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
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50—Control means thereof
    • A61H2201/5007—Control means thereof computer controlled
    • 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

Landscapes

  • Health & Medical Sciences (AREA)
  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • General Health & Medical Sciences (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Veterinary Medicine (AREA)
  • Computing Systems (AREA)
  • Pain & Pain Management (AREA)
  • Rehabilitation Therapy (AREA)
  • Epidemiology (AREA)
  • Public Health (AREA)
  • Algebra (AREA)
  • Animal Behavior & Ethology (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Rehabilitation Tools (AREA)

Abstract

本发明提供了一种六自由度的上肢康复机器人控制方法及系统，所述方法包括建立基于所述上肢康复机器人的D‑H坐标系模型，基于所述D‑H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵；基于所述末端执行器的位置利用所述相对矩阵求解出所有所述电机所需运动的角度，并将所有所述电机所需运动的角度形成所需角度集合；通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置；基于所述人体模型的位置、所有所述电机的位置与所有所述关节连杆的位置，排除所述所需角度集合中会碰撞到所述人体模型的角度，形成防碰撞角度集合；利用所述防碰撞角度集合作为对应所述电机的运动角度，提高上肢康复机器人的安全性。

Description

一种六自由度的上肢康复机器人控制方法及系统
技术领域
本发明属于上肢康复技术领域，具体地涉及一种六自由度的上肢康复机器人控制方法及系统。
背景技术
上肢参与了日常活动中的大部分动作，其功能损伤严重影响患者运动能力与生活质量，因此，上肢运动功能训练是脑卒中患者康复过程中极其重要的一环。
针对面向家庭和社区的穿戴式上肢康复机器人在没有医师的帮助下，安全是非常重要的一个环节，患者穿戴上机械臂后对机械臂的控制要避免与人体发生碰撞，对机械臂控制来说应该有合理的控制范围和模式，康复训练包括被动训练和主动训练，针对不同肌力大小患者的康复模式，机械臂展现不同的控制模式，对于不同的控制模式都有着应有的人体碰撞规避方法。
发明内容
为了解决上述技术问题，本发明提供了一种六自由度的上肢康复机器人控制方法及系统，用于解决现有技术中的技术问题。
一方面，该发明提供以下技术方案，一种六自由度的上肢康复机器人控制方法，所述上肢康复机器人包括：基座及设置在所述基座上的若干关节连杆和若干电机，若干所述关节连杆通过所述电机依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述方法包括：
建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵；
基于所述末端执行器的位置利用所述相对矩阵求解出所有所述电机所需运动的角度，并将所有所述电机所需运动的角度形成所需角度集合；
令所述基座为原点建立空间直角坐标系，并建立基于所述原点的人体模型，通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置；
基于所述人体模型的位置、所有所述电机的位置与所有所述关节连杆的位置，排除所述所需角度集合中会碰撞到所述人体模型的角度，形成防碰撞角度集合；
利用所述防碰撞角度集合作为对应所述电机的运动角度，并利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力。
相比现有技术，本申请的有益效果为：通过排除所述所需角度集合中会碰撞到所述人体模型的角度的步骤，可以将所有的电机运行的所有角度中对人体模型会碰撞的角度进行排除，从而使所有电机运行的角度都不好碰撞到人体，防止上肢康复机器人在康复训练过程中对人体发生碰撞，提高上肢康复机器人的安全性；另外通过控制所述上肢康复机器人的阻力的步骤，可以使上肢康复机器人在靠近身体一定范围内时，会受到一定的阻力，随着距离越近，阻力越强，避免碰撞，在远离身体或安全范围内运动时，不受到阻力影响，从而可以提高上肢康复机器人的安全性和实用性。
进一步的，所述利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力的包括：
基于所有所述电机的位置，计算所述电机距离所述人体模型的位置，判断所述电机距离所述人体模型的位置是否小于阈值；
若所述电机距离所述人体模型的位置小于阈值，则获取所述末端执行器的运动方向，并利用磁场定向控制基于所述末端执行器的运动方向，控制所述上肢康复机器人的阻力。
进一步的，所述并利用磁场定向控制基于所述末端执行器的运动方向的步骤包括：
判断所述末端执行器的运动方向是否往靠近所述人体模型的方向运行，若所述末端执行器的运动方向往靠近所述人体模型的方向运行；
则令所述磁场定向控制的刚度系数与阻尼系数基于反比公式增加，并获取所述电机的当前角度，并将所述当前角度作为所述电机的运动角度。
进一步的，所述磁场定向控制包括力位速三者控制公式，所述力位速三者控制公式包括：
所述反比公式包括：
式中，为刚度系数，是比例增益，/>为阻尼系数，是微分增益，/>为电机的电流力矩常数，/>为前馈扭矩，/>为输出扭矩，/>和/>为电机目标位置和电机目标速度，和/>为电机当前实际位置和电机当前实际速度，/>为电机距离人体模型的位置，/>为阈值，/>为常量系数。
进一步的，所述建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵的步骤包括：
建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述上肢康复机器人的各所述关节连杆与各所述关节点的D-H参数；
基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵，基于所述通用齐次变换矩阵确定末端执行器相对于所述基座坐标系的相对矩阵。
进一步的，所述排除所述所需角度集合中会碰撞到所述人体模型的角度的步骤包括；
获取所有所述电机的三维坐标及所述人体模型的三维坐标最大值以及三维坐标最小值，将所述电机的三维坐标分别与所述三维坐标最大值和所述三维坐标最小值进行对比；
若所述电机的三维坐标大于所述三维坐标最小值，且所述电机的三维坐标小于所述三维坐标最大值，则认为该坐标的所述电机与所述人体碰撞，同时将该坐标的所述电机对应的角度从所述所需角度集合中排除。
进一步的，所述通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置的步骤包括：
获取所述电机的当前角度，并选取所述电机对应所述所需角度集合中的角度，基于所述电机的当前角度与所述电机对应的角度求解出所述电机的角度最大值；
根据所有所述电机的角度最大值，统一所有所述电机的最大转动时间；
将所述电机的最大转动时间细分为若干小段时间，并计算出每一所述小段时间所述电机对应旋转的角度；
通过所述相对矩阵正向求解与所述电机对应旋转的角度，获得所有所述电机在所述空间直角坐标系中的位置；
其中，所有所述电机在所述空间直角坐标系中的位置可表示为：
Pos2表示2号电机在空间直角坐标系中的位置，Pos3表示3号电机在空间直角坐标系中的位置，Pos4表示4号电机在空间直角坐标系中的位置，Pos5表示5号电机在空间直角坐标系中的位置，Pos6表示末端执行器在空间直角坐标系中的位置。
第二方面，该发明提供以下技术方案，所述六自由度的上肢康复机器人控制系统，所述上肢康复机器人包括：基座及设置在所述基座上的若干关节连杆和若干电机，若干所述关节连杆通过所述电机依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述系统包括：
求解模块，用于建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵；
所需模块，用于基于所述末端执行器的位置利用所述相对矩阵求解出所有所述电机所需运动的角度，并将所有所述电机所需运动的角度形成所需角度集合；
位置模块，用于令所述基座为原点建立空间直角坐标系，并建立基于所述原点的人体模型，通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置；
防撞模块，用于基于所述人体模型的位置、所有所述电机的位置与所有所述关节连杆的位置，排除所述所需角度集合中会碰撞到所述人体模型的角度，形成防碰撞角度集合；
控制模块，用于利用所述防碰撞角度集合作为对应所述电机的运动角度，并利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力。
第三方面，该发明提供以下技术方案，一种计算机，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述的六自由度的上肢康复机器人控制方法。
第四方面，该发明提供以下技术方案，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上述的六自由度的上肢康复机器人控制方法。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明第一实施例提供的六自由度的上肢康复机器人控制方法的流程图；
图2为本发明第一实施例提供的六自由度的上肢康复机器人控制方法电机加速度的折线图；
图3为本发明第二实施例提供的六自由度的上肢康复机器人控制系统的结构框图；
图4为本发明第三实施例提供的计算机的硬件结构示意图。
以下将结合附图对本发明实施例作进一步说明。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号数据为相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明的实施例，而不能理解为对本发明的限制。
在本发明实施例的描述中，需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
实施例一
在本发明的第一个实施例中，请参阅图1，一种六自由度的上肢康复机器人控制方法，所述上肢康复机器人包括：基座及设置在所述基座上的若干关节连杆和若干电机，若干所述关节连杆通过所述电机依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，包括以下步骤S01至步骤S05：
S01，建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵；
具体的，所述建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵的步骤包括：
建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述上肢康复机器人的各所述关节连杆与各所述关节点的D-H参数；
基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵，基于所述通用齐次变换矩阵确定末端执行器相对于所述基座坐标系的相对矩阵。
具体的，所述通用齐次变换矩阵为：
；
式中，为第/>个关节点与第/>个关节点之间的通用齐次变换矩阵，/>为三角函数/>，/>为三角函数/>，/>和/>为第/>个关节点和第/>关节点对应的关节变量，为第/>个关节点与第/>个关节点之间的距离，/>、/>分别为第/>个关节点、第/>个关节点的扭角，/>为第/>个关节点的偏距；
所述相对矩阵为：
；
为末端执行器相对于基座坐标系的相对矩阵，/>、/>、/>、/>、/>、/>分别为第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点、第四个关节点相对于第三个关节点、第五个关节点相对于第四个关节点、末端执行器相对于第五个关节点的通用齐次变换矩阵，n、o、a代表的是末端关节坐标系相对于基坐标系的姿态，/>表示末端执行器的x轴在基座坐标系下的投影，/>表示末端执行器的y轴在基座坐标系下的投影，/>表示末端执行器的z轴在基座坐标系下的投影，/>代表的是末端关节坐标系相对于基坐标系的位置。
在本实施例中，利用 D-H 法建立了上肢康复机器人运动学坐标系模型，即D-H坐标系模型，各连杆的坐标系 Z 轴方向与关节电机的轴心重合；X轴方向垂直与连杆两端关节轴线的公垂线，方向指向下一个连杆；确定X轴与Z轴，通过右手法则确定Y轴，在确定所有连杆的坐标之后，根据所有连杆的坐标可以求解出各个关节的D-H参数，D-H方法对连杆和关节定义了4个参数具体如下表1所示。
表1
S02，基于所述末端执行器的位置利用所述相对矩阵求解出所有所述电机所需运动的角度，并将所有所述电机所需运动的角度形成所需角度集合；
在本实施例中，电机的数量为五个，已知末端执行器对于5号电机的相对坐标位置如下公式：
则有两两关节之间得相对矩阵推导可得
其中，、/>、/>是关于/>的函数，/>、/>、是关于/>和/>的函数。
按照上述与/>公式依次对矩阵进行相乘，求得/>、/>、及/>的表达式。
根据实际的机器人机械臂结构得到的改进版D-H参数具体如下表2所示；
表2
其中，杆长=280mm，/>=280mm；扭角/>,/>，/>；偏距/>=233.83mm，/>=-146.83mm。
根据改进版D-H参数表已知D-H参数，将数据量代入，根据公式可得：
=/>
已知末端执行器位置：
已知末端执行器对于5号电机的相对坐标位置；
假设确定末端执行器对于4号电机相对坐标位置，可求出所需的角度；
同理确定末端执行器对于3号电机相对坐标位置，可以求出所需的和/>角度关系；
确定末端执行器对于2号电机相对坐标位置，可以求出所需的和/>和/>角度关系；
确定末端执行器对于1号电机相对坐标位置，可以求出所需的和/>和/>和/>角度关系；
已知基座电机1号电机位置固定，可以求出所需的和/>和/>和/>和/>角度关系；
依次循环假设已知求出所有满足条件的角度的解的集合/>，即所需角度集合。
S03，令所述基座为原点建立空间直角坐标系，并建立基于所述原点的人体模型，通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置；
具体的，所述通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置的步骤包括：
获取所述电机的当前角度，并选取所述电机对应所述所需角度集合中的角度，基于所述电机的当前角度与所述电机对应的角度求解出所述电机的角度最大值；
根据所有所述电机的角度最大值，统一所有所述电机的最大转动时间；
将所述电机的最大转动时间细分为若干小段时间，并计算出每一所述小段时间所述电机对应旋转的角度；
通过所述相对矩阵正向求解与所述电机对应旋转的角度，获得所有所述电机在所述空间直角坐标系中的位置；
其中，所有所述电机在所述空间直角坐标系中的位置可表示为：
Pos2表示2号电机在空间直角坐标系中的位置，Pos3表示3号电机在空间直角坐标系中的位置，Pos4表示4号电机在空间直角坐标系中的位置，Pos5表示5号电机在空间直角坐标系中的位置，Pos6表示末端执行器在空间直角坐标系中的位置；
在本实施例中，根据机械臂结构，以基座电机为原点建立空间直角坐标系，在空间直角坐标系中建立人体上半身模型，采集多组人体上半身尺寸数据，将头部与上半身体近似看作两个大小立方体，取数据平均值作为人体尺寸标准模型M，在标准模型M的各个尺寸上分别扩大0.2倍，作为空间中模拟的人体模型，生成空间直角坐标系中的人体位置坐标。
在本实施例中，所述通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置的步骤具体包括：
1.依次取出集合中的每一组运动轨迹的解，对每一组运动轨迹计算；
2.运动轨迹的电机目标角度为，根据传感器可知电机当前角度为/>,i表示第i个电机。
3.求出当前解中所有电机中旋转角度最大的值/>，i取1-5；
；比较/>-/>，取最大值为/>；
4. 设定旋转角度最大的电机的最大转动速度v，最大加速度，加加速度j。电机的运动速度曲线为S型，S型速度规划更柔和，运动过程更平稳，7段S型速度规划由7个过程组成，分别为：T1—加加速度运动，T2—匀加速运动，T3—减加速运动，T4—匀速运动，T5-T7与T1-T3对称，方向相反具体如图2所示；
其中，加速度表达式如下：
分别对应图2中7个阶段的加速度随时间变化的关系。
式中，对应T1阶段加速度变化，加速度a匀速增加，/>对应T2阶段加速度变化，加速度达到最大值，/>对应T3阶段加速度变化，加速度匀速下降，0对应T4阶段加速度变化，T4阶段加速度为0，/>对应T5阶段加速度变化，T5阶段反向加速度匀速增加，/>对应T6阶段加速度变化，T6阶段反向加速度到达最大值，/>对应T7阶段加速度变化，T7阶段反向加速度匀速减小，逐渐到0。/>表示/>，表示在第x阶段已经花费的时间量，/>=jT1= jT3= jT5= jT7；
由速度v与加速度a关系可以推出速度表达式如下：
分别对应7个阶段的速度随时间变化的关系。
对应T1阶段速度大小，速度增加，并且变化量增大，/>对应T2阶段速度大小，速度匀速增加，/>对应T3阶段速度大小，速度增加，变化量减小，/>对应T4阶段速度大小，速度达到最大值，并保持匀速运动 ，/>表示T5阶段速度大小，速度减小，并且变化量增大，/>表示T6阶段速度大小，速度匀速减小，表示T7阶段速度大小，速度减小，并且变化量减小，逐渐到0。
表示第x阶段结束时的速度，/>是T1结束时电机的速度，/>是T2结束时电机的速度，/>是T3结束时电机的速度，/>达到最大速度，/>是T4结束时电机的速度，/>是T5结束时电机的速度，/>是T6结束时电机的速度。
位移s的公式由速度v对时间t的积分可得：
表示位移，/>表示速度，/>表示对时间积分，/>表示初始速度，/>表示初始加速度，j为加加速度。
根据速度v与加速度a关系公式，推导出图2中7段过程中的位移表达式如下：
该公式描述了7个阶段位移随时间变化的关系。
表示T1阶段位移量，/>表示T2阶段位移量，表示T3阶段位移量，/>表示T4阶段位移量，表示T5阶段位移量，/>表示T6阶段位移量，表示T7阶段位移量；
表示第x阶段结束时所运动的角度，/>是T1结束时电机转动的角度，/>是T2结束时电机转动的角度，/>是T3结束时电机转动的角度，/>是T4结束时电机转动的角度，/>是T5结束时电机转动的角度，/>是T6结束时电机转动的角度；
5.由此根据旋转角度最大的值，带入以上公式计算得出所有电机的最大转动时间t，设定所有电机转动时间t，根据时间t和电机的位移角度/>设置其他电机的最大加速度a以达到所有电机的同时启停；
6. 将时长t进行细分成每一个小段,根据位移与时间的公式，计算得出每一小段/>时刻电机旋转角度，通过相邻两所述关节点之间的通用齐次变换矩阵正向求解，获得电机在空间直角坐标系中的位置；
Pos2表示2号电机在空间直角坐标系中的位置，Pos3表示3号电机在空间直角坐标系中的位置，Pos4表示4号电机在空间直角坐标系中的位置，Pos5表示5号电机在空间直角坐标系中的位置，Pos6表示末端在空间直角坐标系中的位置；同时通过齐次变换矩阵正向求解可以求出连杆拐角点或连杆中点在空间直角坐标系中的位置。其中，1号电机连接在基座上，1号电机通过一关节连杆连接2号电机，2号电机通过一关节连杆连接3号电机，3号电机通过一关节连杆连接4号电机，4号电机通过一关节连杆连接5号电机，5号电机通过一关节连杆连接末端（即为末端执行器）。
S04，基于所述人体模型的位置、所有所述电机的位置与所有所述关节连杆的位置，排除所述所需角度集合中会碰撞到所述人体模型的角度，形成防碰撞角度集合；
具体的，所述排除所述所需角度集合中会碰撞到所述人体模型的角度的步骤包括；
获取所有所述电机的三维坐标及所述人体模型的三维坐标最大值以及三维坐标最小值，将所述电机的三维坐标分别与所述三维坐标最大值和所述三维坐标最小值进行对比；
若所述电机的三维坐标大于所述三维坐标最小值，且所述电机的三维坐标小于所述三维坐标最大值，则认为该坐标的所述电机与所述人体碰撞，同时将该坐标的所述电机对应的角度从所述所需角度集合中排除；
在本实施例中，人体模型包括头部和上半身。
最终由上述过程计算集合中所有不会产生碰撞的解/>。
S05，利用所述防碰撞角度集合作为对应所述电机的运动角度，并利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力；
具体的，所述利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力的包括：
基于所有所述电机的位置，计算所述电机距离所述人体模型的位置，判断所述电机距离所述人体模型的位置是否小于阈值；
若所述电机距离所述人体模型的位置小于阈值，则获取所述末端执行器的运动方向，并利用磁场定向控制基于所述末端执行器的运动方向，控制所述上肢康复机器人的阻力。
具体的，所述并利用磁场定向控制基于所述末端执行器的运动方向的步骤包括：
判断所述末端执行器的运动方向是否往靠近所述人体模型的方向运行，若所述末端执行器的运动方向往靠近所述人体模型的方向运行；
则令所述磁场定向控制的刚度系数与阻尼系数基于反比公式增加，并获取所述电机的当前角度，并将所述当前角度作为所述电机的运动角度。
具体的，所述磁场定向控制包括力位速三者控制公式，所述力位速三者控制公式包括：
所述反比公式包括：
式中，为刚度系数，是比例增益，/>为阻尼系数，是微分增益，/>为电机的电流力矩常数，/>为前馈扭矩，/>为输出扭矩，/>和/>为电机目标位置和电机目标速度，/>和/>为电机当前实际位置和电机当前实际速度，/>为电机距离人体模型的位置，/>为阈值，/>为常量系数。
参考转矩=*(设定位置-当前机械位置)+/>*(设定速度-当前机械速度)+前馈扭矩；
设定速度为0的情况下力位速三者控制公式简化为：
控制所述上肢康复机器人的阻力具体实施过程如下：
S51,前馈扭矩设置为电机受到的重力大小与杆长的乘积，作用于电机的重力补偿，电机运动过程中降低因重力因素产生的误差；
S52,通过三维力传感器力的方向进行运动预测机器人末端的运动方向；
S53,跟据上述的机械臂的逆运动求解方法，计算出各个关节电机运动轨迹，将求解出的角度作为FOC控制的电机目标角度，通过传感器获取电机当前角度/>；
S54,实时通过齐次变换矩阵正向求解，获得电机在空间直角坐标系中的位置，计算电机距离头部空间和上半身空间的距离；
S55,判断电机距离头部空间和上半身空间的距离与阈值的大小；
当距离大于设定安全值/>（即阈值），令刚度系数/>和/>为零，参考扭矩就是前馈扭矩，运动过程不受任何阻力影响。
当距离小于设定安全值/>，电机的运动方向远离身体，令刚度系数/>和/>为零，参考扭矩就是前馈扭矩，运动过程不受任何阻力影响。
当距离小于设定安全值/>，电机的运动方向靠近身体，则刚度系数/>和距离/>成反比关系增加，并且将电机目标角度/>设置为获取到的电机当前角度/>。同时按一定比例增加/>的大小，使得在距离身体较近并逐渐碰到身体时受到比较大的阻力，
为常量系数。
至此机器人在靠近身体一定范围内时，会受到一定的阻力，随着距离越近，阻力越强，避免碰撞，在远离身体或安全范围内运动时，不受到阻力影响。
综上，通过排除所述所需角度集合中会碰撞到所述人体模型的角度的步骤，可以将所有的电机运行的所有角度中对人体模型会碰撞的角度进行排除，从而使所有电机运行的角度都不好碰撞到人体，防止上肢康复机器人在康复训练过程中对人体发生碰撞，提高上肢康复机器人的安全性；另外通过控制所述上肢康复机器人的阻力的步骤，可以使上肢康复机器人在靠近身体一定范围内时，会受到一定的阻力，随着距离越近，阻力越强，避免碰撞，在远离身体或安全范围内运动时，不受到阻力影响，从而可以提高上肢康复机器人的安全性和实用性。
实施例二
如图3所示，在本发明的第二个实施例提供了六自由度的上肢康复机器人控制系统，所述上肢康复机器人包括：基座及设置在所述基座上的若干关节连杆和若干电机，若干所述关节连杆通过所述电机依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述系统包括：
求解模块10，用于建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵；
所需模块20，用于基于所述末端执行器的位置利用所述相对矩阵求解出所有所述电机所需运动的角度，并将所有所述电机所需运动的角度形成所需角度集合；
位置模块30，用于令所述基座为原点建立空间直角坐标系，并建立基于所述原点的人体模型，通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置；
防撞模块40，用于基于所述人体模型的位置、所有所述电机的位置与所有所述关节连杆的位置，排除所述所需角度集合中会碰撞到所述人体模型的角度，形成防碰撞角度集合；
控制模块50，用于利用所述防碰撞角度集合作为对应所述电机的运动角度，并利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力。
在一些可选实施例中，所述控制模块50包括：
阻力单元，用于基于所有所述电机的位置，计算所述电机距离所述人体模型的位置，判断所述电机距离所述人体模型的位置是否小于阈值；若所述电机距离所述人体模型的位置小于阈值，则获取所述末端执行器的运动方向，并利用磁场定向控制基于所述末端执行器的运动方向，控制所述上肢康复机器人的阻力。
在一些可选实施例中，所述阻力单元包括：
判断单元，用于判断所述末端执行器的运动方向是否往靠近所述人体模型的方向运行，若所述末端执行器的运动方向往靠近所述人体模型的方向运行；则令所述磁场定向控制的刚度系数与阻尼系数基于反比公式增加，并获取所述电机的当前角度，并将所述当前角度作为所述电机的运动角度。
在一些可选实施例中，所述控制模块50包括：
公式模块，用于所述磁场定向控制包括力位速三者控制公式，所述力位速三者控制公式包括：
所述反比公式包括：
式中，为刚度系数，是比例增益，/>为阻尼系数，是微分增益，/>为电机的电流力矩常数，/>为前馈扭矩，/>为输出扭矩，/>和/>为电机目标位置和电机目标速度，和/>为电机当前实际位置和电机当前实际速度，/>为电机距离人体模型的位置，/>为阈值，/>为常量系数。
在一些可选实施例中，所述求解模块10包括：
矩阵单元，用于建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述上肢康复机器人的各所述关节连杆与各所述关节点的D-H参数；基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵，基于所述通用齐次变换矩阵确定末端执行器相对于所述基座坐标系的相对矩阵。
在一些可选实施例中，所述求解模块10包括：
获取所有所述电机的三维坐标及所述人体模型的三维坐标最大值以及三维坐标最小值，将所述电机的三维坐标分别与所述三维坐标最大值和所述三维坐标最小值进行对比；
若所述电机的三维坐标大于所述三维坐标最小值，且所述电机的三维坐标小于所述三维坐标最大值，则认为该坐标的所述电机与所述人体碰撞，同时将该坐标的所述电机对应的角度从所述所需角度集合中排除。
在一些可选实施例中，所述防撞模块40包括：
位置单元，用于获取所述电机的当前角度，并选取所述电机对应所述所需角度集合中的角度，基于所述电机的当前角度与所述电机对应的角度求解出所述电机的角度最大值；根据所有所述电机的角度最大值，统一所有所述电机的最大转动时间；将所述电机的最大转动时间细分为若干小段时间，并计算出每一所述小段时间所述电机对应旋转的角度；通过所述相对矩阵正向求解与所述电机对应旋转的角度，获得所有所述电机在所述空间直角坐标系中的位置；其中，所有所述电机在所述空间直角坐标系中的位置可表示为：
Pos2表示2号电机在空间直角坐标系中的位置，Pos3表示3号电机在空间直角坐标系中的位置，Pos4表示4号电机在空间直角坐标系中的位置，Pos5表示5号电机在空间直角坐标系中的位置，Pos6表示末端执行器在空间直角坐标系中的位置。
本发明实施例所提供的六自由度的上肢康复机器人控制系统，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，系统实施例部分未提及之处，可参考前述方法实施例中相应内容。
实施例三
如图4所示，在本发明的第三实施例中，本发明实施例提供以下技术方案，一种计算机，包括存储器202、处理器201以及存储在所述存储器202上并可在所述处理器201上运行的计算机程序，所述处理器201执行所述计算机程序时实现如上所述的六自由度的上肢康复机器人控制方法。
具体的，上述处理器201可以包括中央处理器（CPU），或者特定集成电路（Application Specific Integrated Circuit，简称为ASIC），或者可以被配置成实施本申请实施例的一个或多个集成电路。
其中，存储器202可以包括用于数据或指令的大容量存储器。举例来说而非限制，存储器202可包括硬盘驱动器（Hard Disk Drive，简称为HDD）、软盘驱动器、固态驱动器（Solid State Drive，简称为SSD）、闪存、光盘、磁光盘、磁带或通用串行总线（UniversalSerial Bus，简称为USB）驱动器或者两个或更多个以上这些的组合。在合适的情况下，存储器202可包括可移除或不可移除（或固定）的介质。在合适的情况下，存储器202可在数据处理装置的内部或外部。在特定实施例中，存储器202是非易失性（Non-Volatile）存储器。在特定实施例中，存储器202包括只读存储器（Read-Only Memory，简称为ROM）和随机存取存储器（Random Access Memory，简称为RAM）。在合适的情况下，该ROM可以是掩模编程的ROM、可编程ROM（Programmable Read-Only Memory，简称为PROM）、可擦除PROM（ErasableProgrammable Read-Only Memory，简称为EPROM）、电可擦除PROM（Electrically ErasableProgrammable Read-Only Memory，简称为EEPROM）、电可改写ROM（ElectricallyAlterable Read-Only Memory，简称为EAROM）或闪存（FLASH）或者两个或更多个以上这些的组合。在合适的情况下，该RAM可以是静态随机存取存储器（Static Random-AccessMemory，简称为SRAM）或动态随机存取存储器（Dynamic Random Access Memory，简称为DRAM），其中，DRAM可以是快速页模式动态随机存取存储器（Fast Page Mode DynamicRandom Access Memory，简称为FPMDRAM）、扩展数据输出动态随机存取存储器（ExtendedDate Out Dynamic Random Access Memory，简称为EDODRAM）、同步动态随机存取内存（Synchronous Dynamic Random-Access Memory，简称SDRAM）等。
存储器202可以用来存储或者缓存需要处理和/或通信使用的各种数据文件，以及处理器201所执行的可能的计算机程序指令。
处理器201通过读取并执行存储器202中存储的计算机程序指令，以实现上述六自由度的上肢康复机器人控制方法。
在其中一些实施例中，计算机还可包括通信接口203和总线200。其中，如图4所示，处理器201、存储器202、通信接口203通过总线200连接并完成相互间的通信。
通信接口203用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。通信接口203还可以实现与其他部件例如：外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。
总线200包括硬件、软件或两者，将计算机的部件彼此耦接在一起。总线200包括但不限于以下至少之一：数据总线（Data Bus）、地址总线（Address Bus）、控制总线（ControlBus）、扩展总线（Expansion Bus）、局部总线（Local Bus）。举例来说而非限制，总线200可包括图形加速接口（Accelerated Graphics Port，简称为AGP）或其他图形总线、增强工业标准架构（Extended Industry Standard Architecture，简称为EISA）总线、前端总线（FrontSide Bus，简称为FSB）、超传输（Hyper Transport，简称为HT）互连、工业标准架构（Industry Standard Architecture，简称为ISA）总线、无线带宽（InfiniBand）互连、低引脚数（Lo万 Pin Count，简称为LPC）总线、存储器总线、微信道架构（Micro ChannelArchitecture，简称为MCA）总线、外围组件互连（Peripheral Component Interconnect，简称为PCI）总线、PCI-Express（PCI-X）总线、串行高级技术附件（Serial AdvancedTechnology Attachment，简称为SATA）总线、视频电子标准协会局部（Video ElectronicsStandards Association Local Bus，简称为VLB）总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线200可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线，但本申请考虑任何合适的总线或互连。
实施例四
在本发明的第四实施例中，结合上述的六自由度的上肢康复机器人控制方法，本发明实施例提供以下技术方案，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述的六自由度的上肢康复机器人控制方法。
本领域技术人员可以理解，在流程图中数据为或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列数据表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备（如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统）使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
可读介质的更具体的示例（非穷尽性列数据表）包括以下：具有一个或多个布线的电连接部（电子装置），便携式计算机盘盒（磁装置），随机存取存储器（RAM），只读存储器（ROM），可擦除可编辑只读存储器（EPROM或闪速存储器），光纤装置，以及便携式光盘只读存储器（CDROM）。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或它们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列（PGA），现场可编程门阵列（FPGA）等。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种六自由度的上肢康复机器人控制方法，其特征在于，所述上肢康复机器人包括：基座及设置在所述基座上的若干关节连杆和若干电机，若干所述关节连杆通过所述电机依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述方法包括：
建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵；
基于所述末端执行器的位置利用所述相对矩阵求解出所有所述电机所需运动的角度，并将所有所述电机所需运动的角度形成所需角度集合；
令所述基座为原点建立空间直角坐标系，并建立基于所述原点的人体模型，通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置；
基于所述人体模型的位置、所有所述电机的位置与所有所述关节连杆的位置，排除所述所需角度集合中会碰撞到所述人体模型的角度，形成防碰撞角度集合；
利用所述防碰撞角度集合作为对应所述电机的运动角度，并利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力。
2.根据权利要求1所述的六自由度的上肢康复机器人控制方法，其特征在于，所述利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力的包括：
基于所有所述电机的位置，计算所述电机距离所述人体模型的位置，判断所述电机距离所述人体模型的位置是否小于阈值；
若所述电机距离所述人体模型的位置小于阈值，则获取所述末端执行器的运动方向，并利用磁场定向控制基于所述末端执行器的运动方向，控制所述上肢康复机器人的阻力。
3.根据权利要求2所述的六自由度的上肢康复机器人控制方法，其特征在于，所述并利用磁场定向控制基于所述末端执行器的运动方向的步骤包括：
判断所述末端执行器的运动方向是否往靠近所述人体模型的方向运行，若所述末端执行器的运动方向往靠近所述人体模型的方向运行；
则令所述磁场定向控制的刚度系数与阻尼系数基于反比公式增加，并获取所述电机的当前角度，并将所述当前角度作为所述电机的运动角度。
4.根据权利要求1所述的六自由度的上肢康复机器人控制方法，其特征在于，所述磁场定向控制包括力位速三者控制公式，所述力位速三者控制公式包括：
所述反比公式包括：
式中，为刚度系数，是比例增益，/>为阻尼系数，是微分增益，/>为电机的电流力矩常数，/>为前馈扭矩，/>为输出扭矩，/>和/>为电机目标位置和电机目标速度，/>和为电机当前实际位置和电机当前实际速度，/>为电机距离人体模型的位置，/>为阈值，为常量系数。
5.根据权利要求1所述的六自由度的上肢康复机器人控制方法，其特征在于，所述建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵的步骤包括：
建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述上肢康复机器人的各所述关节连杆与各所述关节点的D-H参数；
基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵，基于所述通用齐次变换矩阵确定末端执行器相对于所述基座坐标系的相对矩阵。
6.根据权利要求1所述的六自由度的上肢康复机器人控制方法，其特征在于，所述排除所述所需角度集合中会碰撞到所述人体模型的角度的步骤包括；
获取所有所述电机的三维坐标及所述人体模型的三维坐标最大值以及三维坐标最小值，将所述电机的三维坐标分别与所述三维坐标最大值和所述三维坐标最小值进行对比；
若所述电机的三维坐标大于所述三维坐标最小值，且所述电机的三维坐标小于所述三维坐标最大值，则认为该坐标的所述电机与所述人体碰撞，同时将该坐标的所述电机对应的角度从所述所需角度集合中排除。
7.根据权利要求1所述的六自由度的上肢康复机器人控制方法，其特征在于，所述通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置的步骤包括：
获取所述电机的当前角度，并选取所述电机对应所述所需角度集合中的角度，基于所述电机的当前角度与所述电机对应的角度求解出所述电机的角度最大值；
根据所有所述电机的角度最大值，统一所有所述电机的最大转动时间；
将所述电机的最大转动时间细分为若干小段时间，并计算出每一所述小段时间所述电机对应旋转的角度；
通过所述相对矩阵正向求解与所述电机对应旋转的角度，获得所有所述电机在所述空间直角坐标系中的位置；
其中，所有所述电机在所述空间直角坐标系中的位置表示为：
Pos2表示2号电机在空间直角坐标系中的位置，Pos3表示3号电机在空间直角坐标系中的位置，Pos4表示4号电机在空间直角坐标系中的位置，Pos5表示5号电机在空间直角坐标系中的位置，Pos6表示末端执行器在空间直角坐标系中的位置，、/>、/>、/>、/>、/>分别为第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点、第四个关节点相对于第三个关节点、第五个关节点相对于第四个关节点、末端执行器相对于第五个关节点的通用齐次变换矩阵。
8.一种六自由度的上肢康复机器人控制系统，其特征在于，所述上肢康复机器人包括：基座及设置在所述基座上的若干关节连杆和若干电机，若干所述关节连杆通过所述电机依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述系统包括：
求解模块，用于建立基于所述上肢康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解末端执行器相对于所述基座坐标系的相对矩阵；
所需模块，用于基于所述末端执行器的位置利用所述相对矩阵求解出所有所述电机所需运动的角度，并将所有所述电机所需运动的角度形成所需角度集合；
位置模块，用于令所述基座为原点建立空间直角坐标系，并建立基于所述原点的人体模型，通过所述相对矩阵正向求解获得所有所述电机在所述空间直角坐标系中的位置；
防撞模块，用于基于所述人体模型的位置、所有所述电机的位置与所有所述关节连杆的位置，排除所述所需角度集合中会碰撞到所述人体模型的角度，形成防碰撞角度集合；
控制模块，用于利用所述防碰撞角度集合作为对应所述电机的运动角度，并利用磁场定向控制基于所述末端执行器的运动方向与所有所述电机的位置，控制所述上肢康复机器人的阻力。
9.一种计算机，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述的六自由度的上肢康复机器人控制方法。
10.一种存储介质，其特征在于，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的六自由度的上肢康复机器人控制方法。
CN202410390169.6A 2024-04-02 2024-04-02 一种六自由度的上肢康复机器人控制方法及系统 Active CN117959146B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410390169.6A CN117959146B (zh) 2024-04-02 2024-04-02 一种六自由度的上肢康复机器人控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410390169.6A CN117959146B (zh) 2024-04-02 2024-04-02 一种六自由度的上肢康复机器人控制方法及系统

Publications (2)

Publication Number Publication Date
CN117959146A true CN117959146A (zh) 2024-05-03
CN117959146B CN117959146B (zh) 2024-06-25

Family

ID=90851900

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410390169.6A Active CN117959146B (zh) 2024-04-02 2024-04-02 一种六自由度的上肢康复机器人控制方法及系统

Country Status (1)

Country Link
CN (1) CN117959146B (zh)

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105904462A (zh) * 2016-05-27 2016-08-31 湖南科瑞特科技股份有限公司 一种六自由度智能机器人本体的控制方法
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN108983785A (zh) * 2018-08-06 2018-12-11 上海电气集团股份有限公司 康复机器人的防碰撞方法及系统
WO2019119724A1 (zh) * 2017-12-21 2019-06-27 东南大学 基于力觉信息和姿态信息的肢体运动意图理解与上肢康复训练机器人控制方法
CN113011458A (zh) * 2021-02-19 2021-06-22 华南理工大学 负载机动型外骨骼人体运动意图识别方法及外骨骼系统
CN113442118A (zh) * 2021-06-30 2021-09-28 山东大学 一种可穿戴外肢体机器人碰撞响应控制方法及系统
CN113618742A (zh) * 2021-08-24 2021-11-09 深圳市优必选科技股份有限公司 一种机器人避障方法、装置和机器人
CN113722864A (zh) * 2021-09-13 2021-11-30 哈工大机器人（合肥）国际创新研究院 一种7自由度冗余机械臂逆运动学求解方法及系统
CN114028153A (zh) * 2021-09-18 2022-02-11 深圳华鹊景医疗科技有限公司 一种康复机器人及其控制方法
CN116570461A (zh) * 2023-04-14 2023-08-11 深圳华鹊景医疗科技有限公司 九自由度上肢康复机器人
CN116725810A (zh) * 2023-02-09 2023-09-12 布法罗机器人科技（成都）有限公司 上肢康复训练设备的控制方法、装置及上肢康复训练设备
CN117428791A (zh) * 2023-12-21 2024-01-23 江西求是高等研究院 一种用于肩部四轴康复机器人的逆运动学求解方法及系统

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105904462A (zh) * 2016-05-27 2016-08-31 湖南科瑞特科技股份有限公司 一种六自由度智能机器人本体的控制方法
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
WO2019119724A1 (zh) * 2017-12-21 2019-06-27 东南大学 基于力觉信息和姿态信息的肢体运动意图理解与上肢康复训练机器人控制方法
CN108983785A (zh) * 2018-08-06 2018-12-11 上海电气集团股份有限公司 康复机器人的防碰撞方法及系统
CN113011458A (zh) * 2021-02-19 2021-06-22 华南理工大学 负载机动型外骨骼人体运动意图识别方法及外骨骼系统
CN113442118A (zh) * 2021-06-30 2021-09-28 山东大学 一种可穿戴外肢体机器人碰撞响应控制方法及系统
CN113618742A (zh) * 2021-08-24 2021-11-09 深圳市优必选科技股份有限公司 一种机器人避障方法、装置和机器人
CN113722864A (zh) * 2021-09-13 2021-11-30 哈工大机器人（合肥）国际创新研究院 一种7自由度冗余机械臂逆运动学求解方法及系统
CN114028153A (zh) * 2021-09-18 2022-02-11 深圳华鹊景医疗科技有限公司 一种康复机器人及其控制方法
CN116725810A (zh) * 2023-02-09 2023-09-12 布法罗机器人科技（成都）有限公司 上肢康复训练设备的控制方法、装置及上肢康复训练设备
CN116570461A (zh) * 2023-04-14 2023-08-11 深圳华鹊景医疗科技有限公司 九自由度上肢康复机器人
CN117428791A (zh) * 2023-12-21 2024-01-23 江西求是高等研究院 一种用于肩部四轴康复机器人的逆运动学求解方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
杜 豪: "虚拟现实技术在柔性上肢康复机器人中的应用", 计算机工程与应用, vol. 56, no. 24, 13 January 2020 (2020-01-13) *

Also Published As

Publication number Publication date
CN117959146B (zh) 2024-06-25

Similar Documents

Publication Publication Date Title
CN111538949B (zh) 冗余机器人逆运动学求解方法、装置和冗余机器人
CN109719725B (zh) 一种机械臂运动规划的方法、机械臂及机器人
WO2023024317A1 (zh) 一种机器人避障方法、装置和机器人
CN111185909B (zh) 机器人运行工况获取方法、装置、机器人及存储介质
CN115946131B (zh) 一种柔性关节机械臂运动控制仿真计算方法及装置
CN113031579A (zh) 双足机器人的行走控制方法、装置及双足机器人
US20240025038A1 (en) Robot control method, legged robot using the same, and computer-readable storage medium
CN117428791B (zh) 一种用于肩部四轴康复机器人的逆运动学求解方法及系统
CN114932549A (zh) 空间冗余机械臂的运动规划方法与装置
CN117959146B (zh) 一种六自由度的上肢康复机器人控制方法及系统
CN113858207B (zh) 人形机器人的倒地控制方法、装置、电子设备及存储介质
CN109227539B (zh) 一种用于空间柔性机械臂的碰撞力最小化方法
WO2023123911A1 (zh) 机器人碰撞检测方法、装置、电子设备及存储介质
CN113442118B (zh) 一种可穿戴外肢体机器人碰撞响应控制方法及系统
CN113070877B (zh) 一种面向七轴机械臂视觉示教的可变姿态映射方法
EP4205916A1 (en) Learning device, learning method, learning program, control device, control method, and control program
CN113246124B (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
CN112008731B (zh) 一种空中作业机器人的柔顺控制方法、装置、终端、系统及可读存储介质
JP2022550231A (ja) 状態決定方法および装置、ロボット、記憶媒体並びにコンピュータプログラム
CN118161835B (zh) 一种机械臂助力控制方法、康复机器人、计算机及介质
CN116643501B (zh) 稳定性约束下的空中作业机器人可变阻抗控制方法及系统
CN114536335B (zh) 机器人机械结构的控制方法、系统、电子设备及存储介质
CN117085301B (zh) 基于虚拟现实的康复机器人多反馈康复训练方法及系统
CN118046394B (zh) 一种遥操作的运动控制方法、装置、系统及存储介质
US20240181633A1 (en) Robot control method, robot, and computer-readable storage medium

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