[发明专利]一种面向SRS仿人手臂的自运动角计算方法

权利要求书

1.一种面向SRS仿人手臂的自运动角计算方法，其特征在于：它包括如下步骤：

步骤一：建立SRS构型冗余度机械臂的数学模型，根据结构特点建立肩关节、肘关节、腕关节坐标系；

步骤二：由当前状态下的自运动角通过PID算法计算给定工具坐标系位姿时的自运动角；

步骤三：由步骤二给定工具坐标系位姿时的自运动角和工具坐标系位姿计算关节空间运动角度；

步骤四：将步骤三的求解结果传递给下位机控制器，驱动机械臂实现给定工具坐标系位姿的运动；

其中，步骤二中所述PID算法具体过程如下：

1)由当前状态的自运动角θ_n和给定的执行末端点的位姿确定各个关节运动角度；

2)将得到的关节运动角度，计算腕关节姿态矩阵的Z轴与腕关节中心点矢量构成的平面与当前状态下的大臂小臂平面之间的夹角α_n；

3)新的自运动角θ_n+1由如下公式：θ_n+1＝θ_n+P*α_n+I*(θ_n–θ₀)求出；

式中符号说明如下：

θ_n+1为新的自运动角，θ_n为上一次计算得到的自运动角，θ₀为初始状态的自运动角，P、I为可调节参数；

4)在内存中存储自运动角θ_n+1；

该算法需给定初始状态时的自运动角θ₀，由初始状态时的自运动角θ₀按照上面1)-4)所述递推。

2.根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于：步骤一所述“建立SRS构型冗余度机械臂的数学模型”，其实现过程如下：测量肩关节中心到肘关节中心的距离和肘关节中心到腕关节中心的距离，其几何模型为一个三动杆机构。

3.根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于：基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，所述SRS构型S为球副，R转动副，所述肩关节、肘关节、腕关节分别对应S球副、R转动副、S球副。

4.根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于：基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，坐标系建立如下所述：将肩关节转动中心、轴关节转动中心的连线称为大臂，肘关节转动中心、腕关节转动中心的连线称为前臂，在初始状态下肩关节坐标系Z轴与大臂平行，并由肩关节转动中心指向肘关节转动中心；Y轴平行于肘关节转动轴线，肘关节坐标系Z轴与小臂平行，并由肘关节转动中心指向腕关节转动中心，Y轴为肘关节转动轴线方向，腕关节处坐标系为肘关节坐标系沿前臂平移。

5.根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于：基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，参数自运动角θ的含义：腕关节中心点和执行末端姿态已知时，肩关节中心点、肘关节中心点和腕关节中心点组成的平面绕腕关节中心点矢量转动，称其为自运动平面，对应转角称为自运动角，并规定当工具坐标点为(x,y,z)时，自运动角为零时自运动平面的法矢方向为(-y,x,0)；其余自运动平面对应的自运动角大小为自运动平面法矢与自运动角为零时自运动平面法矢方向的夹角。

6.根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于：基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，由自运动角和执行末端位姿计算关节角度的步骤如下：

步骤1)由腕关节中心点求出自运动角为0时大臂小臂的法向矢量N；若腕关节中心点为(x,y,z)，则上面所述法向矢量N为(-y,x,0)；

步骤2)将步骤1)中所述法向矢量N绕腕关节中心点(x,y,z)转动自运动角值θ_n得到大臂小臂平面的法矢量N₁；

步骤3)通过余弦定理求出肩关节中心点，肘关节中心点和腕关节中心点构成的三角形的三个角度，大臂与肩关节、腕关节连线的夹角用符号α表示，大臂小臂之间的夹角用符号β表示；

步骤4)将腕关节中心点矢量绕步骤2)求出的法矢量N₁转动步骤3)求出的角度α，得出大臂的矢量方向AB，AB方向即肩关节转动后的Z轴方向，步骤2)求出的法矢量N₁为肩关节转动后的Y轴，转动后肩关节的坐标确定；

步骤5)肘关节的转动角度即步骤3)中所求角度π-β；

步骤6)运动后腕关节的坐标是通过肩关节的旋转矩阵乘以肘关节的旋转矩阵，然后对两矩阵的乘积求逆，最后将求得的逆矩阵乘以给定执行末端的姿态矩阵得到。