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

说明书

技术领域

本发明涉及冗余度机械臂运动学及控制领域，具体来说，是一种面向SRS仿人手臂的自运动角计算方法。

背景技术

机械臂是实现机器人系统主体与末端执行器连接的重要工具，机械臂的活动范围、负载能力直接影响机器人末端的作业能力。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角度。给定冗余自由度机械臂的末端位姿，关节空间有无数个点与之对应，这些点的集合是关节空间的一个n-m维流形。冗余自由度机械臂可以在这个流形内自由变动而不影响末端位姿，这个变动称为冗余自由度机器人的自运动。传统的冗余自由度机械臂的逆运动学解法如梯度投影法耗时较长，或人为退化一个关节角转化为无冗余自由度问题。

发明内容

为了解决上述问题，本发明提出一种面向SRS仿人手臂的自运动角计算方法。

本发明通过以下技术方案予以实现：

一种面向SRS仿人手臂的自运动角计算方法，它包括如下步骤：

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

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

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

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

其中，步骤一所述“建立SRS构型冗余度机械臂的数学模型”，其实现过程如下：将测量肩关节中心到肘关节中心的距离和肘关节中心到腕关节中心的距离，其几何模型为一个三动杆机构。

其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，所述SRS构型S为球副，R转动副，文中所述中肩关节、肘关节、腕关节分别对应S球副、R转动副、S球副。

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

其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，参数自运动角θ的含义：腕关节中心点和执行末端姿态已知时，肩关节中心点、肘关节中心点和腕关节中心点组成的平面可以绕腕关节中心点矢量转动，称其为自运动平面，对应转角称为自运动角，并规定当工具坐标点为(x,y,z)时，自运动角为零时自运动平面的法矢方向为(-y,x,0)。其余自运动平面对应的自运动角大小为自运动平面法矢与自运动角为零时自运动平面法矢方向的夹角。

其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，其中步骤二中所述PID算法具体过程如下：

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

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

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

说明如下：

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

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

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

其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，由自运动角和执行末端位姿计算关节角度的步骤如下：

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

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