[发明专利]一种七自由度遥操作机械臂系统运动学求解方法及系统

权利要求书

1.一种七自由度遥操作机械臂系统运动学求解方法，其特征在于，所述方法包括：

S1、采集获取被测机械臂系统的连杆坐标数据，据以建立七自由度遥操作机械臂D-H坐标系；

S2、获取七自由度遥操作机械臂的不少于2组的连杆位姿变换数据，利用D-H矩阵法，以预置关系构建逻辑处理所述七自由度遥操作机械臂D-H坐标系及所述连杆位姿变换数据，据以建立所述七自由度遥操作机械臂的正运动学方程，以表征机械臂连杆坐标关系；

S3、实时采集所述七自由度遥操作机械臂的预置机械臂角度，将所述预置机械臂角度设置为已知值；

S4、利用矩阵反变换方法，以预置反变换逻辑处理所述机械臂连杆坐标关系以及预置机械臂角度，据以求解得到七自由度机械臂待求角度，以作为七自由度遥操作机械臂运动学求解结果。

2.根据权利要求1所述的七自由度机械臂逆运动学求解方法，其特征在于，所述步骤S1包括：

S11、获取七自由度机械臂关节角度及移动距离及方向数据；

S12、以机械臂基座为起始端建立基坐标0坐标系，顺次编号至所述七自由度机械臂的末端处，据以建立7号坐标系，以作为所述七自由度遥操作机械臂D-H坐标系；

S13、根据所述七自由度遥操作机械臂D-H坐标系，以预置逻辑获取机械臂坐标系变换矩阵：

3.根据权利要求1所述的七自由度机械臂逆运动学求解方法，其特征在于，所述步骤S2包括：

S21、处理所述七自由度遥操作机械臂D-H坐标系及所述连杆位姿变换数据，求解得出机械臂各个相邻两个连杆之间的坐标系矩阵

S22、连乘处理所述坐标系矩阵，据以得到机械臂末端坐标系相对于机械臂基坐标系的变换矩阵

4.根据权利要求1所述的七自由度机械臂逆运动学求解方法，其特征在于，所述步骤S3中，在关节角度解算过程中，使第二角度尽量少运动，以将所述第二角度作为已知值，并通过所述七自由度机械臂上的传感器组采集获取所述第二角度的数值，供代入方程以求解所述七自由度机械臂的第一角度、第三角度、第四角度、第五角度、第六角度及第七角度。

5.根据权利要求4所述的七自由度机械臂逆运动学求解方法，其特征在于，所述步骤S4包括：

S41、获取七自由度机械臂位姿数据，据以计算第六关节旋转角度；

S42、根据位姿矩阵计算七自由度机械臂的第七关节旋转角度；

S43、根据位姿矩阵计算七自由度机械臂的第五关节旋转角度；

S44、根据位姿矩阵中的元素等式关系求解得到七自由度机械臂的第三关节旋转角度。

S45、根据位姿矩阵中的元素等式关系求解得到七自由度机械臂的第二、第三及第四关节的旋转角度。