一种面向srs仿人手臂的自运动角计算方法
【技术领域】
[0001] 本发明涉及冗余度机械臂运动学及控制领域，具体来说，是一种面向SRS仿人手 臂的自运动角计算方法。
【背景技术】
[0002] 机械臂是实现机器人系统主体与末端执行器连接的重要工具，机械臂的活动范 围、负载能力直接影响机器人末端的作业能力。冗余度机械臂的逆运动学问题是指已知机 械臂末端位姿，确定机械臂的关节角度。给定冗余自由度机械臂的末端位姿，关节空间有无 数个点与之对应，这些点的集合是关节空间的一个n-m维流形。冗余自由度机械臂可以在 这个流形内自由变动而不影响末端位姿，这个变动称为冗余自由度机器人的自运动。传统 的冗余自由度机械臂的逆运动学解法如梯度投影法耗时较长，或人为退化一个关节角转化 为无冗余自由度问题。
【发明内容】
[0003] 为了解决上述问题，本发明提出一种面向SRS仿人手臂的自运动角计算方法。
[0004] 本发明通过以下技术方案予以实现：
[0005] 一种面向SRS仿人手臂的自运动角计算方法，它包括如下步骤：
[0006] 步骤一：建立SRS构型冗余度机械臂的数学模型，根据结构特点建立肩关节、肘关 节、腕关节坐标系；
[0007] 步骤二：由当前状态下的自运动角通过PID算法计算给定工具坐标系位姿时的自 运动角；
[0008] 步骤三：由步骤二的自运动角和工具坐标系位姿计算关节空间运动角度；
[0009] 步骤四：将步骤三的求解结果传递给下位机控制器，驱动机械臂实现给定工具坐 标系位姿的运动。
[0010] 其中，步骤一所述"建立SRS构型冗余度机械臂的数学模型"，其实现过程如下：将 测量肩关节中心到肘关节中心的距离和肘关节中心到腕关节中心的距离，其几何模型为一 个三动杆机构。
[0011] 其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，所述SRS构型S 为球副，R转动副，文中所述中肩关节、肘关节、腕关节分别对应S球副、R转动副、S球副。
[0012] 其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，坐标系建立如 下所述：将肩关节转动中心、轴关节转动中心的连线称为大臂，肘关节转动中心、腕关节转 动中心的连线称为前臂。在初始状态下肩关节坐标系Z轴与大臂平行，并由肩关节转动中 心指向肘关节转动中心；Y轴平行于肘关节转动轴线。肘关节坐标系Z轴与小臂平行，并由 肘关节转动中心指向腕关节转动中心，Y轴为肘关节转动轴线方向。腕关节处坐标系为肘 关节坐标系沿前臂平移。
[0013] 其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，参数自运动 角Θ的含义：腕关节中心点和执行末端姿态已知时，肩关节中心点、肘关节中心点和腕 关节中心点组成的平面可以绕腕关节中心点矢量转动，称其为自运动平面，对应转角称为 自运动角，并规定当工具坐标点为（x，y，z)时，自运动角为零时自运动平面的法矢方向为 (_y, X，0)。其余自运动平面对应的自运动角大小为自运动平面法矢与自运动角为零时自运 动平面法矢方向的夹角。
[0014] 其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，其中步骤二中 所述PID算法具体过程如下：
[0015] 1)由当前状态的自运动角θη和给定的执行末端点的位姿确定各个关节运动角 度；
[0016] 2)将步骤二计算得到的关节角度，计算腕关节姿态矩阵的Z轴与腕关节中心点矢 量构成的平面与大臂小臂平面之间的夹角α η;
[0017] 3)新的自运动角θη+1由如下公式：θ η+1= θ η+Ρ*αη+Ι*(θη- Θ。）求出；式中符 号
[0018] 说明如下：
[0019] θη+1为新的自运动角，θ η为上一次计算得到的自运动角，Θ ^为初始状态的自运 动角，P、I为PI可调节参数。
[0020] 4)在内存中存储自运动角θη+1。
[0021] 该算法需给定初始状态时的自运动角Qtl。由初始状态时的自运动角Qtl按照上 面1)-4)所述递推。
[0022] 其中，基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，由自运动角和 执行末端位姿计算关节角度的步骤如下：
[0023] 步骤1)由腕关节中心点求出自运动角为0时大臂小臂的法向矢量N ;若腕关节中 心点为（X，y, ζ)，则上面所述矢量N为（_y, X，0);
[0024] 步骤2)将步骤1)中所述矢量N绕腕关节中心点（X，y, ζ)转动自运动角值Θ ^导 到大臂小臂平面的法矢量N1;
[0025] 步骤3)通过余弦定理求出肩关节中心点，肘关节中心点和腕关节中心点构成的 三角形的三个角度，大臂与肩关节、腕关节连线的夹角用符号α表示，大臂小臂之间的夹 角用符号β表不。
[0026] 步骤4)将腕关节中心点矢量绕步骤2)求出的法矢量&转动步骤3)求出的角度 α，得出大臂的矢量方向AB，AB方向即肩关节转动后的Z轴方向，步骤2)求出的法矢量N1 为肩关节转动后的Y轴，转动后肩关节的坐标可以确定；
[0027] 步骤5)肘关节的转动角度即步骤3)中所求角度π-β ;
[0028] 步骤6)运动后腕关节的坐标是通过肩关节的旋转矩阵乘以肘关节的旋转矩阵， 然后对两矩阵的乘积求逆，最后将求得的逆矩阵乘以给定执行末端的姿态矩阵得到。
[0029] 本发明的优点在于：
[0030] 给出了一种冗余度机械臂的逆运动学求法，和传统的计算方法相比，算法简单，计 算速度快，可以得到解析解，在降低了求解难度的同时对关节运动做了一定的优化。
【附图说明】
[0031] 图1为初始状态下SRS构型冗余度机械臂机构及坐标系建立示意图；
[0032] 图2为一般状态下SRS构型冗余度机械臂机构及坐标系建立示意图；
[0033] 图3为实施本发明方法使用的冗余度机械臂结构示意图；
[0034] 图4(a)是给定直线轨迹下肩关节X支链的移动量轨迹图
[0035] 图4(b)是给定直线轨迹下肩关节Y支链的移动量轨迹图
[0036] 图4(c)是给定直线轨迹下肩关节Z轴转动量轨迹图
[0037] 图4(d)是给定直线轨迹下肘关节转动量轨迹图
[0038] 图4 (e)是给定直线轨迹下腕关节X支链的移动量轨迹图
[0039] 图4 (f)是给定直线轨迹下腕关节Y支链的移动量轨迹图 [0040] 图4 (g)是给定直线轨迹下腕关节关节Z轴转动量轨迹图 [0041] 图5(a)是给定圆形轨迹下肩关节X支链的移动量轨迹图
[0042] 图5(b)是给定圆形轨迹下肩关节Y支链的移动量轨迹图
[0043] 图5(c)是给定圆形轨迹下肩关节Z轴转动量轨迹图 [0044] 图5 (d)是给定圆形轨迹下肘关节转动量轨迹图
[0045] 图5 (e)是给定圆形轨迹下腕关节X支链的移动量轨迹图
[0046] 图5 (f)是给定圆形轨迹下腕关节Y支链的移动量轨迹图
[0047] 图5 (g)是给定圆形轨迹下腕关节关节Z轴转动量轨迹图
[0048] 图6是本发明流程框图
【具体实施方式】
[0049] 下面将结合附图和实例对本发明作进一步的详细说明。
[0050] 1、图3所示为实现本发明所使用的机械臂，该机械臂为SRS构型七自由度机械臂。 该机械臂肩关节转动中心到肘关节转动中心轴线的距离I1 = 270,肘关节转动中心到腕关 节转动中心的距离I2= 230。
[0051] 2、图4(a-g)、5(a_g)为实现本发明的冗余度机械臂逆运动学关节运动曲线示意 图。在这个具体示例中，机械臂的任务是画一条圆弧和一条直线。根据插值点的位姿，利 用本发明所述逆运动学方法求出各关节的运动角度，将计算得到的结果传送给机械臂控制 器，从而控制机械臂完成末端轨迹任务。
[0052] 其中参数自运动角Θ表示在给定工具坐标系的位姿时，冗余度机械臂可以绕