CN104635762B - 一种面向srs仿人手臂的自运动角计算方法 - Google Patents

Abstract

一种面向SRS仿人手臂的自运动角计算方法，它包括如下步骤：步骤一：建立SRS构型冗余度机械臂的数学模型，根据结构特点建立肩关节、肘关节、腕关节坐标系；步骤二：由当前状态下的自运动角通过PID算法计算给定工具坐标系位姿时的自运动角；步骤三：由步骤二的自运动角和工具坐标系位姿计算关节空间运动角度；步骤四：将步骤三的求解结果传递给下位机控制器，驱动机械臂实现给定工具坐标系位姿的运动。本发明给出了一种冗余度机械臂的逆运动学求法，和传统的计算方法相比，算法简单，计算速度快，可以得到解析解，在降低了求解难度的同时对关节运动做了一定的优化。

Description

一种面向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₁；

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

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

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

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

本发明的优点在于：

给出了一种冗余度机械臂的逆运动学求法，和传统的计算方法相比，算法简单，计算速度快，可以得到解析解，在降低了求解难度的同时对关节运动做了一定的优化。

附图说明

图1为初始状态下SRS构型冗余度机械臂机构及坐标系建立示意图；

图2为一般状态下SRS构型冗余度机械臂机构及坐标系建立示意图；

图3为实施本发明方法使用的冗余度机械臂结构示意图；

图4(a)是给定直线轨迹下肩关节X支链的移动量轨迹图

图4(b)是给定直线轨迹下肩关节Y支链的移动量轨迹图

图4(c)是给定直线轨迹下肩关节Z轴转动量轨迹图

图4(d)是给定直线轨迹下肘关节转动量轨迹图

图4(e)是给定直线轨迹下腕关节X支链的移动量轨迹图

图4(f)是给定直线轨迹下腕关节Y支链的移动量轨迹图

图4(g)是给定直线轨迹下腕关节关节Z轴转动量轨迹图

图5(a)是给定圆形轨迹下肩关节X支链的移动量轨迹图

图5(b)是给定圆形轨迹下肩关节Y支链的移动量轨迹图

图5(c)是给定圆形轨迹下肩关节Z轴转动量轨迹图

图5(d)是给定圆形轨迹下肘关节转动量轨迹图

图5(e)是给定圆形轨迹下腕关节X支链的移动量轨迹图

图5(f)是给定圆形轨迹下腕关节Y支链的移动量轨迹图

图5(g)是给定圆形轨迹下腕关节关节Z轴转动量轨迹图

图6是本发明流程框图

具体实施方式

下面将结合附图和实例对本发明作进一步的详细说明。

1、图3所示为实现本发明所使用的机械臂，该机械臂为SRS构型七自由度机械臂。该机械臂肩关节转动中心到肘关节转动中心轴线的距离l₁＝270，肘关节转动中心到腕关节转动中心的距离l₂＝230。

2、图4(a-g)、5(a-g)为实现本发明的冗余度机械臂逆运动学关节运动曲线示意图。在这个具体示例中，机械臂的任务是画一条圆弧和一条直线。根据插值点的位姿，利用本发明所述逆运动学方法求出各关节的运动角度，将计算得到的结果传送给机械臂控制器，从而控制机械臂完成末端轨迹任务。

其中参数自运动角θ表示在给定工具坐标系的位姿时，冗余度机械臂可以绕肩关节中心点和腕关节中心点的连线转动的角度。腕关节中心点和执行末端姿态已知时，肩关节中心点、肘关节中心点和腕关节中心点组成的平面可以绕腕关节中心点矢量转动，称其为自运动平面，对应转角称为自运动角，并规定当工具坐标点为(x,y,z)时，自运动角为零时自运动平面的法矢方向为(-y,x,0)。自运动角的大小为其余自运动平面对应的自运动角大小为自运动平面法矢与自运动角为零时自运动平面法矢方向的夹角。

3、下面介绍本发明结合实际机构尺寸的逆运动学计算过程及结果：

如图2所示，建立SRS构型冗余度机械臂的数学模型，建立坐标系，当腕关节在初始状态时，腕关节的坐标系由肘关节坐标系平移得到，坐标系必须这样建立才能进一步优化计算；其中肩关节转动中心为A，肘关节转动中心为B，腕关节转动中心为C，AB之间的距离为l₁，BC之间的距离为l₂，AC之间的距离为d。

如图1所示，初始状态下的机械臂状态。机械臂上电后初始化，初始位置自运动角θ₀的数值为零。给定一条运动轨迹的插补点序列0，1，2，3…。由初始位置自运动角和插补点0计算关节空间的运动角度。设上一步计算的腕关节姿态矩阵的Z轴与腕关节中心点矢量所在的平面M，计算M与大臂小臂平面的夹角α₁。新的自转角新的自运动角θ₁由如下公式：θ₁＝θ₀+P*α₁+I*(θ₀–θ₀)求出，其中P＝0.2，I＝0.1。通过调节PI可以改善关节空间的运动平滑性。存储当前插补点0处的自运动角θ₁。已知n处自运动角时，由插补点n处自运动角和插补点n+1处给定末端位姿计算插补点n+1处关节空间的运动角度。设上一步计算的腕关节姿态矩阵的Z轴与腕关节中心点矢量所在的平面M，计算M与大臂小臂平面的夹角α_n+1。插补点n+1处的自运动角θ_n+2由如下公式：θ_n+1＝θ_n+P*α_n+1+I*(θ_n–θ₀)求出，其中P＝0.2，I＝0.1。由初始状态的值和递推公式可以求得所有插补点处的关节运动角度。将计算得到的结果传递给下位机控制器，驱动机械臂完成给定的末端轨迹。

上文所述给定自运动角θ和执行末端位姿时，计算关节空间转动角度的步骤如下：

1)设腕关节中心点矢量AC为(x,y,z)，则自转角为0时大臂小臂平面ABC的法向量N为(-y,x,0)。

2)将1)中所述矢量N绕腕关节中心点矢量AC转动自运动角值θ得到大臂小臂平面ABC的法矢量N₁；

3)大臂长度l₁和小臂长度l₂已知，已知AC为(x,y,z)，求得已知三角形ABC三个边的长度，由余弦定理求出图2所示α、β角。

4)步骤2)求出的平面ABC法矢量N₁为肩关节转动后的Y轴在原肩关节坐标系下的坐标。将腕关节中心点矢量AC绕平面ABC的法矢量N₁转动步骤3)求出的角度α，得出大臂的矢量方向AB。将某矢量绕定轴转动一个角度可以采用四元数的方法，是一种常规方法，这里不做陈述。

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

6)4)中求出的转动后肩关节的坐标系按照[x,y,z]写出，即为肩关节的旋转矩阵R₁。其中x为X轴方向的单位矢量，y为Y轴方向的单位矢量，z为Z轴方向的单位矢量。肘关节的旋转矩阵R₂通过绕肘关节轴线方向旋转π-β角，即可求出。绕空间任意方向的转轴转动一个角度的旋转矩阵有常用的公式，此处不做叙述。腕关节的旋转矩阵R₃通过给定的末端姿态R可以求出。R₃＝R₂ ^-1*R₁ ^-1*R。至此，各关节的转动量均已求出，再结合特定构型球副的运动学逆解求出肩关节球球副和腕关节球副驱动关节的运动量即可求出。由R₁、R₂、R₃计算出各关节运动量的方法与不同的球副、转动副实现方法有关，且主题无关，这里省略。

图4(a-g)为从点(100,100,300)沿直线运动到(0,0,500)各关节的运动曲线；图5(a-g)为绕x＝20；y＝50；z＝350转动半径为50的四分之一个圆弧转动关节的运动曲线。本算法可以较快的得到SRS构型冗余度机械臂的逆解，并在一定程度上做了运动平滑性的优化。而且，不同尺寸通过可以调节PI的取值调节各关节运动的平滑性。图6是本发明流程框图。

Claims (6)

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)运动后腕关节的坐标是通过肩关节的旋转矩阵乘以肘关节的旋转矩阵，然后对两矩阵的乘积求逆，最后将求得的逆矩阵乘以给定执行末端的姿态矩阵得到。