[发明专利]串联六关节机械手的姿态控制方法及系统

权利要求书

1.一种串联六关节机械手的姿态控制方法，其特征在于，包括：

位姿矩阵计算步骤S1：读取串联六关节机械手的各关节伺服电机角度，根据读取的各关节伺服电机角度，使用连杆变换矩阵进行正解，得到机械手位姿矩阵A₁₆；

实际点计算步骤S2：根据位姿矩阵A₁₆来确定机械手空间坐标X、Y、Z，将位姿矩阵A₁₆中的旋转矩阵转换为四元数q1、q2、q3、q4，得到实际点P1(X，Y，Z，q1，q2，q3，q4)；

设置步骤S3：根据用户手动示教或修改实际点P1得到目标点P2；

前三轴角度计算步骤S4：将P2的四元数转换至旋转矩阵并与XYZ组合成目标点P2的位姿矩阵T₁₆；根据位姿矩阵T₁₆求出前三个关节机械手的轴角度θ₁、θ₂、θ₃；

后三轴角度计算步骤S5：根据轴角度θ₁、θ₂、θ₃得到前三个关节的逆矩阵表达式再计算得到后三个关节机械手的轴角度θ₄、θ₅、θ₆；

发送步骤S6：将得到的各轴角度对应发送至各轴伺服电机；

位姿矩阵计算步骤S1包括：

子步骤S11：根据矩阵计算结果得出连杆变换4x4矩阵A_n表达式：

其中，alpha_n是机械手第n个关节与相邻的后一个关节轴的夹角，a_n是表示机械手第n个关节与相邻的后一个关节轴公垂线的长度，d_n是机械手第n个关节沿两个相邻连杆公共轴线方向的距离，n代表第几个关节；

子步骤S12：将各轴角度代入连杆变换矩阵，经过矩阵乘法计算得出机械手位姿矩阵A₁₆，A₁₆表达式为：

A₁₆＝A₁*A₂*A₃*A₄*A₅*A₆；

再计算得到A₁₆矩阵表达式为：

其中，为机械手在当前位置下的旋转矩阵，px、py、pz分别为当前位置下的三轴坐标。

2.如权利要求1所述的串联六关节机械手的姿态控制方法，其特征在于，实际点计算步骤S2包括：

子步骤S21：由A₁₆矩阵表达式得到旋转矩阵R的表达式和空间坐标，其中，空间坐标中的XYZ分别等于A₁₆中的px、py、pz，

子步骤S22：由四元数转矩阵得到旋转矩阵R的另一表达式：

其中w＝q1，x＝q2，y＝q3，z＝q4；

子步骤S23：由子步骤S21和S22中旋转矩阵R的表达式得到：

ox+ny＝4xy；

ox-ny＝4wz；

nz+ax＝4xz；

nz-ax＝4wy；

ay+oz＝4yz；

ay+oz＝4wx；

由上式计算得到w、x、y、z，从而得到实际点P1(X，Y，Z，q1，q2，q3，q4)。