CN117428791B - 一种用于肩部四轴康复机器人的逆运动学求解方法及系统 - Google Patents

一种用于肩部四轴康复机器人的逆运动学求解方法及系统 Download PDF

Info

Publication number
CN117428791B
CN117428791B CN202311764361.9A CN202311764361A CN117428791B CN 117428791 B CN117428791 B CN 117428791B CN 202311764361 A CN202311764361 A CN 202311764361A CN 117428791 B CN117428791 B CN 117428791B
Authority
CN
China
Prior art keywords
point
joint
shoulder
joint point
axis
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.)
Active
Application number
CN202311764361.9A
Other languages
English (en)
Other versions
CN117428791A (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 CN202311764361.9A priority Critical patent/CN117428791B/zh
Publication of CN117428791A publication Critical patent/CN117428791A/zh
Application granted granted Critical
Publication of CN117428791B publication Critical patent/CN117428791B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • 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
    • A61H1/0281—Shoulder
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • 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

Landscapes

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

Abstract

本发明提供了一种用于肩部四轴康复机器人的逆运动学求解方法及系统，所述方法包括建立D‑H坐标系模型，求解肩部四轴康复机器人的D‑H参数；基于D‑H参数确定通用齐次变换矩阵；基于通用齐次变换矩阵确定相对矩阵；确定肩关节点、肘关节点与腕关节点以及二维坐标，基于二维坐标求解关节点的关节变量；根据关节点的关节变量与相对矩阵确定肩部四轴康复机器人的位置与姿态，本发明可在只给定末端执行器位置的情况下实现精准且快速的对肩部四轴康复机器人的位置与姿态进行求解，以便于肩部四轴康复机器人的使用与观测。

Description

一种用于肩部四轴康复机器人的逆运动学求解方法及系统
技术领域
本发明属于运动求解的技术领域，具体地涉及一种用于肩部四轴康复机器人的逆运动学求解方法及系统。
背景技术
机械臂逆运动学是指根据机械臂末端执行器的位置和姿态，计算机械臂各个关节的角度。机械臂逆运动学的计算方法有多种，其中常用的是解析法和数值法。解析法是一种基于数学公式的方法，通过方程组来计算机械臂各个关节的角度。数值法是一种基于迭代计算的方法，通过不断调整关节角度来逼近目标位置和姿态。机械臂逆运动学的计算结果可以用于机械臂的路径规划和控制。
肩部四轴康复机器人的结构上采用了超冗余自由度，可以在多个维度上进行自由运动，灵活完成多样化、复杂的任务，相对于普通机械臂，超冗余机械臂在工作空间、控制精度、负载能力等方面都具有明显的优势，因此在医疗科研等领域得到了广泛应用，同时肩部四轴康复机器人更是一种能够模拟人类手臂运动的机器人，通过结合虚拟现实技术等多媒体技术，模拟日常生活中的常规运动，恢复关节的协调能力，改善上肢的运动控制。
由于其超冗余的特点，肩部四轴康复机器人运动学逆求解有无穷多组解，因此现有技术无法在只给定末端位置的情况下，对肩部四轴康复机器人的逆运动学进行精确快速的求解。
发明内容
为了解决上述技术问题，本发明提供了一种用于肩部四轴康复机器人的逆运动学求解方法及系统，用于解决现有技术中的技术问题。
一方面，本发明提供以下技术方案，一种用于肩部四轴康复机器人的逆运动学求解方法，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，包括：
建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；
基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；
基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；
确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；
根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态。
相比现有技术，本申请的有益效果为：本发明首先建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；而后基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；然后基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；进而确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；最后根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态，本发明可在只给定末端执行器位置的情况下实现精准且快速的对肩部四轴康复机器人的位置与姿态进行求解，以便于肩部四轴康复机器人的使用与观测。
较佳的，在所述基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵的步骤中，所述通用齐次变换矩阵为：
；
式中，表示第个关节点与第个关节点之间的通用齐次变换矩阵，为第个关节点对应的关节变量，为第个关节点与第个关节点之间的距离，、分别为第个关节点、第个关节点的扭角，为第个关节点的偏距。
较佳的，在所述基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵的步骤中，所述相对矩阵为：
；
为末端执行器相对于基座坐标系的相对矩阵，、、、、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点、第四个关节点相对于第三个关节点、第五个关节点相对于第四个关节点、末端执行器相对于第五个关节点的通用齐次变换矩阵，、、表示末端执行器的x轴在基座坐标系下的投影，、、表示末端执行器的y轴在基座坐标系下的投影，、、表示末端执行器的z轴在基座坐标系下的投影，、、表示末端执行器相对于基座坐标系的位置。
较佳的，所述确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量的步骤包括：
将末端执行器所对应的关节点作为腕关节点C，将与末端执行器通过关节连杆连接的关节点作为肘关节点B，将其余关节点作为肩关节点A；
在所述腕关节点C与所述肩关节点A之前确定中间点O，以使直线OB垂直于直线AC，在所述肘关节点B后方确定一辅助点P，以使所述肘关节点B、所述肩关节点A与所述辅助点P共面；
将所述中间点O、所述肘关节点B投影到直线AC所在的平面上，并确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标；
基于所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标，确定所述肩关节点A的三维坐标；
基于所述肩关节点A的三维坐标求解所述关节点的关节变量。
较佳的，在所述确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标的步骤中，所述肩关节点A的二维坐标为：
；
所述肘关节点B的二维坐标为：
；
所述腕关节点C的二维坐标为：
；
式中，为所述肩关节点A与所述肘关节点B之间的距离，为所述肘关节点B与所述腕关节点C之间的距离，为所述肩关节点A与所述腕关节点C之间的距离。
较佳的，所述基于所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标，确定所述肩关节点A的三维坐标的步骤包括：
确定中间点O在设定三维空间中的三维坐标：
；
式中，为由腕关节点C指向肩关节点A的向量，为肩关节点A的三维坐标；
确定所述中间点O在设定三维空间内的三维朝向：
；
；
；
式中，、、分别为所述中间点O在设定三维空间内的x轴朝向、y轴朝向、Z轴朝向，为由肩关节点A指向腕关节点C的向量，为由辅助点P指向腕关节点C的向量，为向量的模；
基于所述中间点O在设定三维空间内的三维朝向确定二维空间的中心点O相对于设定三维空间的变换矩阵：
；
式中，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离；
基于所述变换矩阵确定所述肩关节点A的三维坐标：
。
较佳的，其特征在于，所述基于所述肩关节点A的三维坐标求解所述关节点的关节变量的步骤包括：
将所述肩关节点A的三维坐标进行转化，以得到设定三维坐标，将所述肩关节点A的设定三维坐标进行第一次分解：
；
；
；
；
式中，分别为肩关节点A的x轴、y轴、z轴坐标，、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点的通用齐次变换矩阵，、、分别为第三个关节点对应的关节变量的第一函数、第二函数、第三函数，为第三个关节点与第四个关节点之间的距离，为第三个关节点的扭角，为第四个关节点的偏距；
对所述肩关节点A的设定三维坐标进行第二次分解：
；
；
；
；
式中，、分别为第一个、第二个关节点对应的关节变量，、、分别为的第一函数、第二函数、第三函数，为第一个关节点的偏距；
设定为一个范围在的已知量，并基于两次分解后的设定三维坐标的坐标以及求解各个关节点的关节变量：
；
；
；
；
式中，为由第三关节点与第四关节的连接中点指向肩关节点A的向量，为由肩关节点A指向肘关节点B的向量，为由肘关节点B指向腕关节点C的向量。
第二方面，本发明提供以下技术方案，一种用于肩部四轴康复机器人的逆运动学求解系统，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述系统包括：
参数确定模块，用于建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；
第一矩阵确定模块，用于基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；
第二矩阵确定模块，用于基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；
变量确定模块，用于确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；
位姿确定模块，用于根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态。
第三方面，本发明提供以下技术方案，一种计算机，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述的用于肩部四轴康复机器人的逆运动学求解方法。
第四方面，本发明提供以下技术方案，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上述的用于肩部四轴康复机器人的逆运动学求解方法。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例一提供的用于肩部四轴康复机器人的逆运动学求解方法的流程图；
图2为本发明实施例一提供的肩部四轴康复机器人的结构图；
图3为本发明实施例二提供的用于肩部四轴康复机器人的逆运动学求解系统的结构框图；
图4为本发明另一实施例提供的计算机的硬件结构示意图。
以下将结合附图对本发明实施例作进一步说明。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明的实施例，而不能理解为对本发明的限制。
在本发明实施例的描述中，需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本发明实施例中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明实施例中的具体含义。
实施例一
在本发明的实施例一中，如图1、图2所示，一种用于肩部四轴康复机器人的逆运动学求解方法，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，包括：
具体如图2所述，图2中右侧为基座位置，基座上通过第一关节电机连接有第一关节连杆，第一关节连杆通过第二关节电机连接有第二关节连杆，第二关节连杆通过第三关节电机连接有第三关节连杆，第三关节连杆通过第四关节电机连接有第四关节连杆，第四关节连杆通过第五关节电机连接有第五关节连杆，对于关节点而言，两关节连杆之间即关节电机的位置处即为关节点，同时对于第五关节连杆而言，其远离第五关节电机的一端也为关节点，因此对于该肩部四轴康复机器人而言，其具备六个关节点、五个关节电机以及五根关节连杆，且按照连接关系，将基座处的关节点设为第一关节点，第五关节连杆其远离第五关节电机的一端为第六关节点，其余关节点按照连接关系依次排列。
S1、建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；
具体的，对于本发明中的肩部四轴康复机器人而言，首先使用D-H 法建立了肩部四轴康复机器人的运动学坐标系模型，即D-H坐标系模型，且按照上述方法对关节点与关节连杆进行依次编号，且各连杆的坐标系 Z 轴方向与关节电机的轴心重合；垂直与连杆两端关节轴线的公垂线为连杆坐标系的 X 轴，方向指向下一个连杆，公垂线与 Z轴的交点为坐标系原点；坐标系的 Y 轴由 X 轴和 Z 轴确定，在确定所有连杆的坐标之后，便可按照下面流程建立相邻两关节点之间的相对关系：
1、绕轴旋转角，使轴转到与同一平面内；
2、沿轴平移一距离，把移到与同一直线上；
3、沿轴平移一距离，把连杆 i-1 的坐标系移动到使其原点与连杆 i 坐标系原点重合的地方；
4、绕旋转角，使移到与同一直线上；
按上述步骤操作之后，便可求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数，具体如下表所示。
S2、基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；
其中，所述通用齐次变换矩阵为：
；
式中，表示第个关节点与第个关节点之间的通用齐次变换矩阵，为第个关节点对应的关节变量，为第个关节点与第个关节点之间的距离，、分别为第个关节点、第个关节点的扭角，为第个关节点的偏距。
S3、基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；
其中，在所述基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵的步骤中，所述相对矩阵为：
；
为末端执行器相对于基座坐标系的相对矩阵，、、、、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点、第四个关节点相对于第三个关节点、第五个关节点相对于第四个关节点、末端执行器相对于第五个关节点的通用齐次变换矩阵，、、表示末端执行器的x轴在基座坐标系下的投影，、、表示末端执行器的y轴在基座坐标系下的投影，、、表示末端执行器的z轴在基座坐标系下的投影，、、表示末端执行器相对于基座坐标系的位置。
S4、确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；
其中，所述步骤S4包括：
S41、将末端执行器所对应的关节点作为腕关节点C，将与末端执行器通过关节连杆连接的关节点作为肘关节点B，将其余关节点作为肩关节点A；
具体的，对于肩部四轴康复机器人的六个关节点而言，其前四个关节点对应的关节电机的轴心为同一点，因此可将前四个关节点看成为一个点，即为肩关节点A，而之后的第五个关节点即为肘关节点B，第六个点为腕关节点C，且腕关节点C即为末端执行器所在的点。
S42、在所述腕关节点C与所述肩关节点A之前确定中间点O，以使直线OB垂直于直线AC，在所述肘关节点B后方确定一辅助点P，以使所述肘关节点B、所述肩关节点A与所述辅助点P共面；
具体的，根据机械臂关节点结构，通常将辅助点P约束到放置在肘关节点B后面的定位器，从而当进行反向求解控制轴关节旋转时，肘关节点B不会发生异常翻转，根据辅助点P，确定肘部和末端的姿态，根据上述条件求解出肩部四轴康复机器人各关节变量，从而驱动关节电机，最终使肩部四轴康复机器人末端的位姿符合开始的要求。
S43、将所述中间点O、所述肘关节点B投影到直线AC所在的平面上，并确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标；
具体的，如果不知道辅助点P点的位置信息，那么肘关节点B可以出现在垂直于辅助点P平面且经过O-B所在的平面上，因此为了便于计算将空间中的点进行投影到A-C的平面上，进行肘关节点B求解；
对于投影之后的所述肩关节点A、所述肘关节点B、所述腕关节点C与中间点O而言，中间点O处于二维坐标轴的原点位置，肩关节点A、腕关节点C处于x轴上，肘关节点B处于y轴上，因此可知：
；
其中，为肩关节点A、腕关节点C之间的长度，、为肩关节点A的横、纵坐标，、为腕关节点C的横、纵坐标；
；
其中，为∠BAC的余弦值，同时根据、、以及便可确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标；
其中，所述肩关节点A的二维坐标为：
；
所述肘关节点B的二维坐标为：
；
所述腕关节点C的二维坐标为：
；
式中，为所述肩关节点A与所述肘关节点B之间的距离，为所述肘关节点B与所述腕关节点C之间的距离，为所述肩关节点A与所述腕关节点C之间的距离。
S44、基于所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标，确定所述肩关节点A的三维坐标；
具体的，在实际的使用情况下，肩关节点A的坐标为三维形式，因此需要将步骤S43求得的二维坐标转换为三维坐标，因此可通过获取中间点O二维到三维坐标之间的转换关系，进而将肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标转换为三维。
其中，所述步骤S44包括：
S441、确定中间点O在设定三维空间中的三维坐标：
；
式中，为由腕关节点C指向肩关节点A的向量，为肩关节点A的三维坐标。
S442、确定所述中间点O在设定三维空间内的三维朝向：
；
；
；
式中，、、分别为所述中间点O在设定三维空间内的x轴朝向、y轴朝向、Z轴朝向，为由肩关节点A指向腕关节点C的向量，为由辅助点P指向腕关节点C的向量，为向量的模。
S443、基于所述中间点O在设定三维空间内的三维朝向确定二维空间的中心点O相对于设定三维空间的变换矩阵：
；
式中，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离；
其中，对于、、而言，其均为在三维空间中的单位向量，其在x、y、z轴上均存在对应的值，因此根据其分别在x、y、z轴上的投影值可确定对应的二维至三维的变换矩阵。
S444、基于所述变换矩阵确定所述肩关节点A的三维坐标：
。
其中，同理可得，所述肘关节点B的三维坐标与所述腕关节点C的三维坐标分别为：
；
。
S45、基于所述肩关节点A的三维坐标求解所述关节点的关节变量；
其中，所述步骤S45包括：
S451、将所述肩关节点A的三维坐标进行转化，以得到设定三维坐标，将所述肩关节点A的设定三维坐标进行第一次分解：
；
；
；
；
式中，分别为肩关节点A的x轴、y轴、z轴坐标，、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点的通用齐次变换矩阵，、、分别为第三个关节点对应的关节变量的第一函数、第二函数、第三函数，为第三个关节点与第四个关节点之间的距离，为第三个关节点的扭角，为第四个关节点的偏距；
具体的，根据上一步中求得的所述肩关节点A的三维坐标，可将其转化为的表达形式，以便于后续计算，同时根据步骤S3中所提到的相对矩阵，因此可知，且在实际情况下，为已知数，，将其代入上述表达式，即可得到、、的具体表达式。
S452、对所述肩关节点A的设定三维坐标进行第二次分解：
；
；
；
；
式中，、分别为第一个、第二个关节点对应的关节变量，、、分别为的第一函数、第二函数、第三函数，为第一个关节点的偏距；
具体的，在进行两次分解之后，便可将、的表达式进行转化，以便于后续的关节变量求解的过程。
S453、设定为一个范围在的已知量，并基于两次分解后的设定三维坐标的坐标以及求解各个关节点的关节变量：
；
；
；
；
式中，为由第三关节点与第四关节的连接中点指向肩关节点A的向量，为由肩关节点A指向肘关节点B的向量，为由肘关节点B指向腕关节点C的向量；
需要说明的是，在本发明中，关节变量有6个，在步骤S453中已经求解得到其中的四个，即、、、，其中，为一个范围在的已知量，而对于而言，其无需求解，其为末端执行器的关节变量，同时对于第三关节点与第四关节的连接中点而言，其可根据相对矩阵正向求解得到。
S5、根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态。
具体的，在确定了关节变量之后，同时各个关节点的坐标也求解得到，之后将其代入到相对矩阵中的中，即可解算得到所述肩部四轴康复机器人的位置与姿态。
本发明实施例一提供的用于肩部四轴康复机器人的逆运动学求解方法，首先建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；而后基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；然后基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；然后确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；最后根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态，本发明可在只给定末端执行器位置的情况下实现精准且快速的对肩部四轴康复机器人的位置与姿态进行求解，以便于肩部四轴康复机器人的位置与姿态的使用与观测。
实施例二
如图3所示，在本发明的实施例二提供了一种用于肩部四轴康复机器人的逆运动学求解系统，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述系统包括：
参数确定模块1，用于建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；
第一矩阵确定模块2，用于基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；
第二矩阵确定模块3，用于基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；
变量确定模块4，用于确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；
位姿确定模块5，用于根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态。
所述变量确定模块4包括：
关节点确定子模块，用于将末端执行器所对应的关节点作为腕关节点C，将与末端执行器通过关节连杆连接的关节点作为肘关节点B，将其余关节点作为肩关节点A；
辅助点确定子模块，用于在所述腕关节点C与所述肩关节点A之前确定中间点O，以使直线OB垂直于直线AC，在所述肘关节点B后方确定一辅助点P，以使所述肘关节点B、所述肩关节点A与所述辅助点P共面；
二维坐标确定子模块，用于将所述中间点O、所述肘关节点B投影到直线AC所在的平面上，并确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标；
三维坐标确定子模块，用于基于所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标，确定所述肩关节点A的三维坐标；
关节变量确定子模块，用于基于所述肩关节点A的三维坐标求解所述关节点的关节变量。
所述三维坐标确定子模块包括：
第一确定单元，用于确定中间点O在设定三维空间中的三维坐标：
；
式中，为由腕关节点C指向肩关节点A的向量，为肩关节点A的三维坐标；
第二确定单元，用于确定所述中间点O在设定三维空间内的三维朝向：
；
；
；
式中，、、分别为所述中间点O在设定三维空间内的x轴朝向、y轴朝向、Z轴朝向，为由肩关节点A指向腕关节点C的向量，为由辅助点P指向腕关节点C的向量，为向量的模；
变换矩阵确定单元，用于基于所述中间点O在设定三维空间内的三维朝向确定二维空间的中心点O相对于设定三维空间的变换矩阵：
；
式中，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离；
第三确定单元，用于基于所述变换矩阵确定所述肩关节点A的三维坐标：
。
所述关节变量确定子模块包括：
第一分解单元，用于将所述肩关节点A的三维坐标进行转化，以得到设定三维坐标，将所述肩关节点A的设定三维坐标进行第一次分解：
；
；
；
；
式中，分别为肩关节点A的x轴、y轴、z轴坐标，、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点的通用齐次变换矩阵，、、分别为第三个关节点对应的关节变量的第一函数、第二函数、第三函数，为第三个关节点与第四个关节点之间的距离，为第三个关节点的扭角，为第四个关节点的偏距；
第二分解单元，用于对所述肩关节点A的设定三维坐标进行第二次分解：
；
；
；
；
式中，、分别为第一个、第二个关节点对应的关节变量，、、分别为的第一函数、第二函数、第三函数，为第一个关节点的偏距；
求解单元，用于设定为一个范围在的已知量，并基于两次分解后的设定三维坐标的坐标以及求解各个关节点的关节变量：
；
；
；
；
式中，为由第三关节点与第四关节的连接中点指向肩关节点A的向量，为由肩关节点A指向肘关节点B的向量，为由肘关节点B指向腕关节点C的向量。
在本发明的另一些实施例中，本发明实施例提供以下技术方案，一种计算机，包括存储器102、处理器101以及存储在所述存储器102上并可在所述处理器101上运行的计算机程序，所述处理器101执行所述计算机程序时实现如上所述的用于肩部四轴康复机器人的逆运动学求解方法。
具体的，上述处理器101可以包括中央处理器（CPU），或者特定集成电路（Application Specific Integrated Circuit，简称为ASIC），或者可以被配置成实施本申请实施例的一个或多个集成电路。
其中，存储器102可以包括用于数据或指令的大容量存储器。举例来说而非限制，存储器102可包括硬盘驱动器（Hard Disk Drive，简称为HDD）、软盘驱动器、固态驱动器（Solid State Drive，简称为SSD）、闪存、光盘、磁光盘、磁带或通用串行总线（UniversalSerial Bus，简称为USB）驱动器或者两个或更多个以上这些的组合。在合适的情况下，存储器102可包括可移除或不可移除（或固定）的介质。在合适的情况下，存储器102可在数据处理装置的内部或外部。在特定实施例中，存储器102是非易失性（Non-Volatile）存储器。在特定实施例中，存储器102包括只读存储器（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）等。
存储器102可以用来存储或者缓存需要处理和/或通信使用的各种数据文件，以及处理器101所执行的可能的计算机程序指令。
处理器101通过读取并执行存储器102中存储的计算机程序指令，以实现上述用于肩部四轴康复机器人的逆运动学求解方法。
在其中一些实施例中，计算机还可包括通信接口103和总线100。其中，如图4所示，处理器101、存储器102、通信接口103通过总线100连接并完成相互间的通信。
通信接口103用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。通信接口103还可以实现与其他部件例如：外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。
总线100包括硬件、软件或两者，将计算机设备的部件彼此耦接在一起。总线100包括但不限于以下至少之一：数据总线（Data Bus）、地址总线（Address Bus）、控制总线（Control Bus）、扩展总线（Expansion Bus）、局部总线（Local Bus）。举例来说而非限制，总线100可包括图形加速接口（Accelerated Graphics Port，简称为AGP）或其他图形总线、增强工业标准架构（Extended Industry Standard Architecture，简称为EISA）总线、前端总线（Front Side Bus，简称为FSB）、超传输（Hyper Transport，简称为HT）互连、工业标准架构（Industry Standard Architecture，简称为ISA）总线、无线带宽（InfiniBand）互连、低引脚数（Low Pin Count，简称为LPC）总线、存储器总线、微信道架构（Micro ChannelArchitecture，简称为MCA）总线、外围组件互连（Peripheral Component Interconnect，简称为PCI）总线、PCI-Express（PCI-X）总线、串行高级技术附件（Serial AdvancedTechnology Attachment，简称为SATA）总线、视频电子标准协会局部（Video ElectronicsStandards Association Local Bus，简称为VLB）总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线100可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线，但本申请考虑任何合适的总线或互连。
该计算机可以基于获取到用于肩部四轴康复机器人的逆运动学求解系统，执行本申请的用于肩部四轴康复机器人的逆运动学求解方法，从而实现肩部四轴康复机器人的逆运动学求解。
在本发明的再一些实施例中，结合上述的用于肩部四轴康复机器人的逆运动学求解方法，本发明实施例提供以下技术方案，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述的用于肩部四轴康复机器人的逆运动学求解方法。
本领域技术人员可以理解，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备（如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统）使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
可读介质的更具体的示例（非穷尽性列表）包括以下：具有一个或多个布线的电连接部（电子装置），便携式计算机盘盒（磁装置），随机存取存储器（RAM），只读存储器（ROM），可擦除可编辑只读存储器（EPROM或闪速存储器），光纤装置，以及便携式光盘只读存储器（CDROM）。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或它们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列（PGA），现场可编程门阵列（FPGA）等。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种用于肩部四轴康复机器人的逆运动学求解方法，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，其特征在于，包括：
建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；
基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；
基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；
确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；
根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态；
在所述基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵的步骤中，所述通用齐次变换矩阵为：
；
式中，表示第个关节点与第个关节点之间的通用齐次变换矩阵，为第个关节点对应的关节变量，为第个关节点与第个关节点之间的距离，、分别为第个关节点、第个关节点的扭角，为第个关节点的偏距；
在所述基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵的步骤中，所述相对矩阵为：
；
为末端执行器相对于基座坐标系的相对矩阵，、、、、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点、第四个关节点相对于第三个关节点、第五个关节点相对于第四个关节点、末端执行器相对于第五个关节点的通用齐次变换矩阵，、、表示末端执行器的x轴在基座坐标系下的投影，、、表示末端执行器的y轴在基座坐标系下的投影，、、表示末端执行器的z轴在基座坐标系下的投影，、、表示末端执行器相对于基座坐标系的位置；
所述确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量的步骤包括：
将末端执行器所对应的关节点作为腕关节点C，将与末端执行器通过关节连杆连接的关节点作为肘关节点B，将其余关节点作为肩关节点A；
在所述腕关节点C与所述肩关节点A之前确定中间点O，以使直线OB垂直于直线AC，在所述肘关节点B后方确定一辅助点P，以使所述肘关节点B、所述肩关节点A与所述辅助点P共面；
将所述中间点O、所述肘关节点B投影到直线AC所在的平面上，并确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标；
基于所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标，确定所述肩关节点A的三维坐标；
基于所述肩关节点A的三维坐标求解所述关节点的关节变量；
在所述确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标的步骤中，所述肩关节点A的二维坐标为：
；
所述肘关节点B的二维坐标为：
；
所述腕关节点C的二维坐标为：
；
式中，为所述肩关节点A与所述肘关节点B之间的距离，为所述肘关节点B与所述腕关节点C之间的距离，为所述肩关节点A与所述腕关节点C之间的距离；
所述基于所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标，确定所述肩关节点A的三维坐标的步骤包括：
确定中间点O在设定三维空间中的三维坐标：
；
式中，为由腕关节点C指向肩关节点A的向量，为肩关节点A的三维坐标；
确定所述中间点O在设定三维空间内的三维朝向：
；
；
；
式中，、、分别为所述中间点O在设定三维空间内的x轴朝向、y轴朝向、Z轴朝向，为由肩关节点A指向腕关节点C的向量，为由辅助点P指向腕关节点C的向量，为向量的模；
基于所述中间点O在设定三维空间内的三维朝向确定二维空间的中心点O相对于设定三维空间的变换矩阵：
；
式中，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离；
基于所述变换矩阵确定所述肩关节点A的三维坐标：
；
所述基于所述肩关节点A的三维坐标求解所述关节点的关节变量的步骤包括：
将所述肩关节点A的三维坐标进行转化，以得到设定三维坐标，将所述肩关节点A的设定三维坐标进行第一次分解：
；
；
；
；
式中，分别为肩关节点A的x轴、y轴、z轴坐标，、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点的通用齐次变换矩阵，、、分别为第三个关节点对应的关节变量的第一函数、第二函数、第三函数，为第三个关节点与第四个关节点之间的距离，为第三个关节点的扭角，为第四个关节点的偏距；
对所述肩关节点A的设定三维坐标进行第二次分解：
；
；
；
；
式中，、分别为第一个、第二个关节点对应的关节变量，、、分别为的第一函数、第二函数、第三函数，为第一个关节点的偏距；
设定为一个范围在的已知量，并基于两次分解后的设定三维坐标的坐标以及求解各个关节点的关节变量：
；
；
；
；
式中，为由第三关节点与第四关节的连接中点指向肩关节点A的向量，为由肩关节点A指向肘关节点B的向量，为由肘关节点B指向腕关节点C的向量。
2.一种用于肩部四轴康复机器人的逆运动学求解系统，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，其特征在于，所述系统包括：
参数确定模块，用于建立基于所述肩部四轴康复机器人的D-H坐标系模型，基于所述D-H坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的D-H参数；
第一矩阵确定模块，用于基于各所述关节连杆与各所述关节点的D-H参数确定相邻两所述关节点之间的通用齐次变换矩阵；
第二矩阵确定模块，用于基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；
变量确定模块，用于确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；
位姿确定模块，用于根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态；
其中，所述通用齐次变换矩阵为：
；
式中，表示第个关节点与第个关节点之间的通用齐次变换矩阵，为第个关节点对应的关节变量，为第个关节点与第个关节点之间的距离，、分别为第个关节点、第个关节点的扭角，为第个关节点的偏距；
其中，所述相对矩阵为：
；
为末端执行器相对于基座坐标系的相对矩阵，、、、、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点、第四个关节点相对于第三个关节点、第五个关节点相对于第四个关节点、末端执行器相对于第五个关节点的通用齐次变换矩阵，、、表示末端执行器的x轴在基座坐标系下的投影，、、表示末端执行器的y轴在基座坐标系下的投影，、、表示末端执行器的z轴在基座坐标系下的投影，、、表示末端执行器相对于基座坐标系的位置；
所述变量确定模块包括：
关节点确定子模块，用于将末端执行器所对应的关节点作为腕关节点C，将与末端执行器通过关节连杆连接的关节点作为肘关节点B，将其余关节点作为肩关节点A；
辅助点确定子模块，用于在所述腕关节点C与所述肩关节点A之前确定中间点O，以使直线OB垂直于直线AC，在所述肘关节点B后方确定一辅助点P，以使所述肘关节点B、所述肩关节点A与所述辅助点P共面；
二维坐标确定子模块，用于将所述中间点O、所述肘关节点B投影到直线AC所在的平面上，并确定所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标；
三维坐标确定子模块，用于基于所述肩关节点A、所述肘关节点B与所述腕关节点C的二维坐标，确定所述肩关节点A的三维坐标；
关节变量确定子模块，用于基于所述肩关节点A的三维坐标求解所述关节点的关节变量；
所述三维坐标确定子模块包括：
第一确定单元，用于确定中间点O在设定三维空间中的三维坐标：
；
式中，为由腕关节点C指向肩关节点A的向量，为肩关节点A的三维坐标；
第二确定单元，用于确定所述中间点O在设定三维空间内的三维朝向：
；
；
；
式中，、、分别为所述中间点O在设定三维空间内的x轴朝向、y轴朝向、Z轴朝向，为由肩关节点A指向腕关节点C的向量，为由辅助点P指向腕关节点C的向量，为向量的模；
变换矩阵确定单元，用于基于所述中间点O在设定三维空间内的三维朝向确定二维空间的中心点O相对于设定三维空间的变换矩阵：
；
式中，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离；
第三确定单元，用于基于所述变换矩阵确定所述肩关节点A的三维坐标：
；
所述关节变量确定子模块包括：
第一分解单元，用于将所述肩关节点A的三维坐标进行转化，以得到设定三维坐标，将所述肩关节点A的设定三维坐标进行第一次分解：
；
；
；
；
式中，分别为肩关节点A的x轴、y轴、z轴坐标，、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点的通用齐次变换矩阵，、、分别为第三个关节点对应的关节变量的第一函数、第二函数、第三函数，为第三个关节点与第四个关节点之间的距离，为第三个关节点的扭角，为第四个关节点的偏距；
第二分解单元，用于对所述肩关节点A的设定三维坐标进行第二次分解：
；
；
；
；
式中，、分别为第一个、第二个关节点对应的关节变量，、、分别为的第一函数、第二函数、第三函数，为第一个关节点的偏距；
求解单元，用于设定为一个范围在的已知量，并基于两次分解后的设定三维坐标的坐标以及求解各个关节点的关节变量：
；
；
；
；
式中，为由第三关节点与第四关节的连接中点指向肩关节点A的向量，为由肩关节点A指向肘关节点B的向量，为由肘关节点B指向腕关节点C的向量。
3.一种计算机，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1所述的用于肩部四轴康复机器人的逆运动学求解方法。
4.一种存储介质，其特征在于，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1所述的用于肩部四轴康复机器人的逆运动学求解方法。
CN202311764361.9A 2023-12-21 2023-12-21 一种用于肩部四轴康复机器人的逆运动学求解方法及系统 Active CN117428791B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311764361.9A CN117428791B (zh) 2023-12-21 2023-12-21 一种用于肩部四轴康复机器人的逆运动学求解方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311764361.9A CN117428791B (zh) 2023-12-21 2023-12-21 一种用于肩部四轴康复机器人的逆运动学求解方法及系统

Publications (2)

Publication Number Publication Date
CN117428791A CN117428791A (zh) 2024-01-23
CN117428791B true CN117428791B (zh) 2024-03-01

Family

ID=89546433

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311764361.9A Active CN117428791B (zh) 2023-12-21 2023-12-21 一种用于肩部四轴康复机器人的逆运动学求解方法及系统

Country Status (1)

Country Link
CN (1) CN117428791B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117959146B (zh) * 2024-04-02 2024-06-25 江西求是高等研究院 一种六自由度的上肢康复机器人控制方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103495977A (zh) * 2013-09-29 2014-01-08 北京航空航天大学 一种6r型工业机器人负载识别方法
CN105382835A (zh) * 2015-12-11 2016-03-09 华中科技大学 一种可穿越腕部奇异点的机器人路径规划方法
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN108638069A (zh) * 2018-05-18 2018-10-12 南昌大学 一种机械臂末端精确运动控制方法
CN110757454A (zh) * 2019-10-12 2020-02-07 广州中国科学院先进技术研究所 一种双机器人协同旋转的路径规划方法和装置
CN112597437A (zh) * 2020-12-29 2021-04-02 深圳市优必选科技股份有限公司 机械臂的逆运动学解析方法、装置及设备
CN113510690A (zh) * 2021-03-30 2021-10-19 上海机电工程研究所 四自由度串联机器人逆运动学求解方法和系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111538949B (zh) * 2020-07-10 2020-10-16 深圳市优必选科技股份有限公司 冗余机器人逆运动学求解方法、装置和冗余机器人

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103495977A (zh) * 2013-09-29 2014-01-08 北京航空航天大学 一种6r型工业机器人负载识别方法
CN105382835A (zh) * 2015-12-11 2016-03-09 华中科技大学 一种可穿越腕部奇异点的机器人路径规划方法
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN108638069A (zh) * 2018-05-18 2018-10-12 南昌大学 一种机械臂末端精确运动控制方法
CN110757454A (zh) * 2019-10-12 2020-02-07 广州中国科学院先进技术研究所 一种双机器人协同旋转的路径规划方法和装置
CN112597437A (zh) * 2020-12-29 2021-04-02 深圳市优必选科技股份有限公司 机械臂的逆运动学解析方法、装置及设备
CN113510690A (zh) * 2021-03-30 2021-10-19 上海机电工程研究所 四自由度串联机器人逆运动学求解方法和系统

Also Published As

Publication number Publication date
CN117428791A (zh) 2024-01-23

Similar Documents

Publication Publication Date Title
CN117428791B (zh) 一种用于肩部四轴康复机器人的逆运动学求解方法及系统
CN113910219B (zh) 运动臂系统以及控制方法
CN110640747B (zh) 机器人的手眼标定方法、系统、电子设备和存储介质
CN111801198A (zh) 一种手眼标定方法、系统及计算机存储介质
CN110370256B (zh) 机器人及其路径规划方法、装置和控制器
CN109807937B (zh) 一种基于自然场景的机器人手眼标定方法
CN111168719A (zh) 一种基于定位工装的机器人校准方法及系统
WO2022205844A1 (zh) 机器人正运动学求解方法、装置、可读存储介质及机器人
CN110930444B (zh) 一种基于双边优化的点云匹配方法、介质、终端和装置
CN113814988B (zh) 7自由度srs型机械臂逆解的解析方法、装置及电子设备
CN114089316A (zh) 一种激光雷达-惯导的联合标定系统、方法及介质
CN117340879A (zh) 一种基于图优化模型的工业机器人参数辨识方法和系统
CN116141341B (zh) 满足笛卡尔空间约束的五自由度机械臂指向动作实现方法
CN115179288A (zh) 机器人的运动学逆解方法、机器人及计算机可读存储介质
CN113733098B (zh) 机械臂模型位姿计算方法、装置、电子设备和存储介质
CN115890749A (zh) Rcm约束下的内窥镜机器人视觉伺服与优化控制方法、系统及机器人
CN115416025A (zh) 焊接机器人控制方法、装置、焊接机器人和可读介质
CN114536351A (zh) 冗余双臂机器人示教方法、装置、电子设备及系统
Jagersand Image based view synthesis of articulated agents
CN117944033B (zh) 柔性机械臂的控制方法、电子设备及存储介质
CN116652972B (zh) 基于双向贪心搜索算法的串联机器人末端轨迹规划方法
CN117773927A (zh) 机器人脚踝运动控制方法、装置、可读存储介质及机器人
CN117959146B (zh) 一种六自由度的上肢康复机器人控制方法及系统
CN114290331B (zh) 机器人运动控制方法、机器人及计算机可读存储装置
CN117464667B (zh) 七轴机械臂的运动规划方法、装置及机械臂

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