[发明专利]一种七轴工业机械臂的逆运动学求解方法

权利要求书

1.一种七轴工业机械臂的逆运动学求解方法，其特征在于，包括：

步骤1、将虚拟机械臂末端执行器任务设定的起始点和终止点中间对应的位置离散化，获取离散点；

步骤2、建立第一关节角和第七姿态的坐标原点的平面方程，根据每两个离散点得到第一关节角的变化趋势，根据所述变化趋势得到离散点集合中每一点对应的第一关节角的值；

其中，第一平面坐标系为基座底部坐标点、第零姿态的坐标原点以及第一或第二姿态的坐标原点构成的平面；

步骤3、采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T，根据矩阵变换获取第五或第六姿态的坐标原点，再确定位姿矩阵，根据位姿矩阵求出第三姿态的坐标原点；根据第五或第六姿态的坐标原点和第三姿态的坐标原点的距离方程式，求出第二关节角；

步骤4、建立第四关节角、第四关节角的第一特征角以及第四关节角的第二特征角之间的条件关系，根据所述条件关系求解第四关节角；其中，

所述第四关节角的第一特征角为以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的夹角；

所述第四关节角的第二特征角以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的夹角；

步骤5、由正运动学方程的左右两边的第四列对应相等得到的方程得到关于第一关节角、第二关节角、第四关节角以及第三关节角的两个等式，从而求出第三关节角；

步骤6、在上述步骤1至步骤5求解的关节角已知时，利用正运动学确定的位姿矩阵，根据位姿矩阵得到第四姿态的坐标原点坐标，再求出第四姿态的坐标原点和第七姿态的坐标原点之间的空间距离，再由第四姿态的坐标原点、第五或第六姿态的坐标原点和第七姿态的坐标原点构成的三角形中，求出第六关节角的特征角，根据第六关节角的特征角求解第六关节角；其中，第六关节角的特征角为以第五或第六姿态的坐标原点为顶点，与第四姿态的坐标原点、第七姿态的坐标原点形成的夹角；

步骤7、根据第六关节角的求解结果，结合末端坐标的关系，求解第五关节角和第七关节角。

2.根据权利要求1所述的一种七轴工业机械臂的逆运动学求解方法，其特征在于，在规划第一关节角之前，包括：

以虚拟机械臂末端执行器的以基座的底部点O为基坐标点，再另外依次确定七个位姿状态对应的空间坐标系的坐标原点，其中第一姿态和第二姿态公用同一个坐标原点为第一或第二姿态的坐标原点，第五姿态和第六姿态公用同一个坐标原点为第五或第六姿态的坐标原点。

3.根据权利要求1所述的一种七轴工业机械臂的逆运动学求解方法，其特征在于，在建立第一关节角和第七姿态的坐标原点的平面方程之前，旋转第一关节角，将第七姿态的坐标原点旋转至第一平面坐标系内，使第一关节角和第七姿态的坐标原点处于同一平面。

4.根据权利要求3所述的一种七轴工业机械臂的逆运动学求解方法，其特征在于，包括：

所述第四关节角的第一特征角通过以第三姿态的坐标原点为顶点，与第四姿态的坐标原点及第五或第六姿态坐标原点形成的直角三角形中的三角函数求解；

所述第四关节角的第二特征角通过以以第三姿态的坐标原点为顶点，与第一或第二姿态的坐标原点及第五或第六姿态坐标原点形成的三角形的三角函数求解。

5.根据权利要求4所述的一种七轴工业机械臂的逆运动学求解方法，其特征在于，根据第六关节角的特征角求解第六关节角，包括：第六关节角对应为两个解，具体为第六关节角的特征角的补角的正负值。

6.根据权利要求5所述的一种七轴工业机械臂的逆运动学求解方法，其特征在于，根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节角和第七关节角，包括：

当第六关节角不为零时，由末端坐标的关系求出第五关节角，再由末端姿态的关系求出第七关节角；

当第六关节角为零时，机械臂处于奇异位置，不能唯一确定第五关节角和第七关节角，令第五关节角等于零，则可求出第七关节角，得到第五关节角和第七关节角的一组特解。