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

本发明属于运动求解的，具体地涉及一种用于肩部四轴康复机器人的逆运动学求解方法及系统。

背景技术：

1、机械臂逆运动学是指根据机械臂末端执行器的位置和姿态，计算机械臂各个关节的角度。机械臂逆运动学的计算方法有多种，其中常用的是解析法和数值法。解析法是一种基于数学公式的方法，通过方程组来计算机械臂各个关节的角度。数值法是一种基于迭代计算的方法，通过不断调整关节角度来逼近目标位置和姿态。机械臂逆运动学的计算结果可以用于机械臂的路径规划和控制。

2、肩部四轴康复机器人的结构上采用了超冗余自由度，可以在多个维度上进行自由运动，灵活完成多样化、复杂的任务，相对于普通机械臂，超冗余机械臂在工作空间、控制精度、负载能力等方面都具有明显的优势，因此在医疗科研等领域得到了广泛应用，同时肩部四轴康复机器人更是一种能够模拟人类手臂运动的机器人，通过结合虚拟现实技术等多媒体技术，模拟日常生活中的常规运动，恢复关节的协调能力，改善上肢的运动控制。

3、由于其超冗余的特点，肩部四轴康复机器人运动学逆求解有无穷多组解，因此现有技术无法在只给定末端位置的情况下，对肩部四轴康复机器人的逆运动学进行精确快速的求解。

技术实现思路

1、为了解决上述技术问题，本发明提供了一种用于肩部四轴康复机器人的逆运动学求解方法及系统，用于解决现有技术中的技术问题。

2、一方面，本发明提供以下技术方案，一种用于肩部四轴康复机器人的逆运动学求解方法，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，包括：

3、建立基于所述肩部四轴康复机器人的d-h坐标系模型，基于所述d-h坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的d-h参数；

4、基于各所述关节连杆与各所述关节点的d-h参数确定相邻两所述关节点之间的通用齐次变换矩阵；

5、基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；

6、确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；

7、根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态。

8、相比现有技术，本技术的有益效果为：本发明首先建立基于所述肩部四轴康复机器人的d-h坐标系模型，基于所述d-h坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的d-h参数；而后基于各所述关节连杆与各所述关节点的d-h参数确定相邻两所述关节点之间的通用齐次变换矩阵；然后基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；进而确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；最后根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态，本发明可在只给定末端执行器位置的情况下实现精准且快速的对肩部四轴康复机器人的位置与姿态进行求解，以便于肩部四轴康复机器人的使用与观测。

9、较佳的，在所述基于各所述关节连杆与各所述关节点的d-h参数确定相邻两所述关节点之间的通用齐次变换矩阵的步骤中，所述通用齐次变换矩阵为：

10、；

11、式中，表示第个关节点与第个关节点之间的通用齐次变换矩阵，为第个关节点对应的关节变量，为第个关节点与第个关节点之间的距离，、分别为第个关节点、第个关节点的扭角，为第个关节点的偏距。

12、较佳的，在所述基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵的步骤中，所述相对矩阵为：

13、；

14、为末端执行器相对于基座坐标系的相对矩阵，、、、、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点、第四个关节点相对于第三个关节点、第五个关节点相对于第四个关节点、末端执行器相对于第五个关节点的通用齐次变换矩阵，、、表示末端执行器的x轴在基座坐标系下的投影，、、表示末端执行器的y轴在基座坐标系下的投影，、、表示末端执行器的z轴在基座坐标系下的投影，、、表示末端执行器相对于基座坐标系的位置。

15、较佳的，所述确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量的步骤包括：

16、将末端执行器所对应的关节点作为腕关节点c，将与末端执行器通过关节连杆连接的关节点作为肘关节点b，将其余关节点作为肩关节点a；

17、在所述腕关节点c与所述肩关节点a之前确定中间点o，以使直线ob垂直于直线ac，在所述肘关节点b后方确定一辅助点p，以使所述肘关节点b、所述肩关节点a与所述辅助点p共面；

18、将所述中间点o、所述肘关节点b投影到直线ac所在的平面上，并确定所述肩关节点a、所述肘关节点b与所述腕关节点c的二维坐标；

19、基于所述肩关节点a、所述肘关节点b与所述腕关节点c的二维坐标，确定所述肩关节点a的三维坐标；

20、基于所述肩关节点a的三维坐标求解所述关节点的关节变量。

21、较佳的，在所述确定所述肩关节点a、所述肘关节点b与所述腕关节点c的二维坐标的步骤中，所述肩关节点a的二维坐标为：

22、；

23、所述肘关节点b的二维坐标为：

24、；

25、所述腕关节点c的二维坐标为：

26、；

27、式中，为所述肩关节点a与所述肘关节点b之间的距离，为所述肘关节点b与所述腕关节点c之间的距离，为所述肩关节点a与所述腕关节点c之间的距离。

28、较佳的，所述基于所述肩关节点a、所述肘关节点b与所述腕关节点c的二维坐标，确定所述肩关节点a的三维坐标的步骤包括：

29、确定中间点o在设定三维空间中的三维坐标：

30、；

31、式中，为由腕关节点c指向肩关节点a的向量，为肩关节点a的三维坐标；

32、确定所述中间点o在设定三维空间内的三维朝向：

33、；

34、；

35、；

36、式中，、、分别为所述中间点o在设定三维空间内的x轴朝向、y轴朝向、z轴朝向，为由肩关节点a指向腕关节点c的向量，为由辅助点p指向腕关节点c的向量，为向量的模；

37、基于所述中间点o在设定三维空间内的三维朝向确定二维空间的中心点o相对于设定三维空间的变换矩阵：

38、；

39、式中，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离，、、分别为投影在x、y、z轴上的距离；

40、基于所述变换矩阵确定所述肩关节点a的三维坐标：

41、。

42、较佳的，其特征在于，所述基于所述肩关节点a的三维坐标求解所述关节点的关节变量的步骤包括：

43、将所述肩关节点a的三维坐标进行转化，以得到设定三维坐标，将所述肩关节点a的设定三维坐标进行第一次分解：

44、；

45、；

46、；

47、；

48、式中，分别为肩关节点a的x轴、y轴、z轴坐标，、、分别表示第一个关节点相对于基座坐标系、第二个关节点相对于第一个关节点、第三个关节点相对于第二个关节点的通用齐次变换矩阵，、、分别为第三个关节点对应的关节变量的第一函数、第二函数、第三函数，为第三个关节点与第四个关节点之间的距离，为第三个关节点的扭角，为第四个关节点的偏距；

49、对所述肩关节点a的设定三维坐标进行第二次分解：

50、；

51、；

52、；

53、；

54、式中，、分别为第一个、第二个关节点对应的关节变量，、、分别为的第一函数、第二函数、第三函数，为第一个关节点的偏距；

55、设定为一个范围在的已知量，并基于两次分解后的设定三维坐标的坐标以及求解各个关节点的关节变量：

56、；

57、；

58、；

59、；

60、式中，为由第三关节点与第四关节的连接中点指向肩关节点a的向量，为由肩关节点a指向肘关节点b的向量，为由肘关节点b指向腕关节点c的向量。

61、第二方面，本发明提供以下技术方案，一种用于肩部四轴康复机器人的逆运动学求解系统，所述肩部四轴康复机器人包括基座以及设置在所述基座上的若干关节连杆，若干所述关节连杆依次首尾连接，相邻俩所述关节连杆之间、所述基座与所述关节连杆之间均设有关节点，所述系统包括：

62、参数确定模块，用于建立基于所述肩部四轴康复机器人的d-h坐标系模型，基于所述d-h坐标系模型求解所述肩部四轴康复机器人的各所述关节连杆与各所述关节点的d-h参数；

63、第一矩阵确定模块，用于基于各所述关节连杆与各所述关节点的d-h参数确定相邻两所述关节点之间的通用齐次变换矩阵；

64、第二矩阵确定模块，用于基于所述通用齐次变换矩阵确定末端执行器相对于基座坐标系的相对矩阵；

65、变量确定模块，用于确定所述肩部四轴康复机器人的肩关节点、肘关节点与腕关节点以及所述肩关节点、所述肘关节点与腕关节点的二维坐标，基于所述肩关节点、所述肘关节点与腕关节点的二维坐标求解所述关节点的关节变量；

66、位姿确定模块，用于根据所述关节点的关节变量与所述相对矩阵确定所述肩部四轴康复机器人的位置与姿态。

67、第三方面，本发明提供以下技术方案，一种计算机，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述的用于肩部四轴康复机器人的逆运动学求解方法。

68、第四方面，本发明提供以下技术方案，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上述的用于肩部四轴康复机器人的逆运动学求解方法。