一种绳驱动串联关节型蛇形机械臂控制方法与流程

本发明涉及了一种机械臂控制方法，尤其是涉及了一种绳驱动串联关节型蛇形机械臂控制方法。

背景技术：

绳驱动串联关节型蛇形机械臂是一种新兴的应用于狭窄空间作业的仿生机械臂，通过绳索拉动关节对机械臂进行控制，由于驱动机构全部布置在基座上，机械臂上没有任何驱动机构而变得轻便灵活。针对普通机器人难以开展高效作业的狭窄封闭空间环境，机械臂能够通过狭小的孔和裂缝等进入封闭空间内，然后进行激光切割、修补、侦察内部环境、构建内部三维场景等作业，例如在不拆卸外壳的前提下进入飞机油箱进行检修。

由于蛇形机械臂的自由度数量远大于作业过程所需的自由度数量，因此属于超冗余自由度机械臂，利用冗余的多个自由度能够实现灵活地进行路径规划避开障碍物，进入狭窄的空间完成预定的作业任务。

相较于完全柔性的连续型机械臂，串联关节型机械臂具有更高的控制精度，并且负载能力也大大提高，但完全柔性机械臂的控制方法均建立在等曲率光滑曲线等多个假设前提下，串联关节型蛇形机械臂并不满足条件；而目前大多数刚性机械臂的运动学模型均采用dh法进行构建，但对于具有一定柔性的蛇形机械臂，传统的dh法并不适用。

技术实现要素：

为了解决背景技术中存在的问题，从空间几何关系出发进行研究，本发明提出一种绳驱动串联关节型蛇形机械臂控制方法，构建基于齐次变换矩阵的运动学模型并对机械臂进行控制。

如图1所示，本发明针对的机械臂由多个关节串联而成，关节之间使用万向节连接，因而每个关节具有两个弯曲自由度，则包含n个关节的机械臂总共有2n个自由度。一般在机械臂末端的工作空间需要三个移动自由度，或再加上三个转动自由度，而通常使用的由6个关节串联成的机械臂具有12个自由度，因此具备多个冗余自由度。

通过电机或其他驱动机构拉动绳索对每个关节进行驱动，绳索的另一端固定在关节端面上，每个关节由至少三根并联绳索驱动，在万向节的限制下可以由绳索拉动关节转动。由于每个关节需要至少三根绳索驱动，则n个关节至少需要总共3n根绳索驱动。

以总计6个关节，每个关节由3根绳索驱动为例，机械臂每个关节都有18个绳孔，对于与基座相连的第一个关节，其中只有3个绳孔是用于本节的3根驱动绳的固定，其余15根绳索均穿过本节；第二个关节与由第一个关节穿出的3根绳索固定，其余12根绳索穿过本节，以此类推，最后一个关节与穿过前面所有关节的最后3根绳索固定。

本发明采用的技术方案是：

本发明用给定各关节相对于上一关节的两个空间姿态角度，求解所需要的绳索拉动位移使机械臂运动到所有关节的空间姿态角度所确定的整体姿态。

所述的机械臂是由绳索驱动的机械臂，机械臂由多个关节串联而成，关节之间使用万向节连接，绳索连接到各个关节。具体地，所述机械臂可采用发明名称为《可检测关节姿态的高冗余柔性机械臂装置》，申请日为2017.6.21，申请号为201710475066.x的说明书中记载的技术方案。

如图7所示，方法包括：

1)先构建绝对坐标系xyz和每个关节的参考坐标系x’y’z’；

2)对于机械臂各个关节在所需要的空间姿态角度下，计算机械臂每个关节上的空间点位于绝对坐标系下的绝对坐标(x0,y0,z0)，然后以绝对坐标(x0,y0,z0)为依据计算相邻两个关节上各自一个空间点之间的距离，接着利用所有相邻关节的空间点之间的距离计算每根绳索的总长；

3)以此求出所有绳索各自的绳索总长后，控制绳索拉到各自对应的绳索总长，从而控制机械臂运动到所需要的空间姿态角度，实现对机械臂空间姿态角度的控制。

所述的绝对坐标系xyz具体是指：以机械臂的臂基座端面中心为坐标系中心，沿垂直于臂基座端面的方向为y方向，沿竖直方向为z方向，再根据右手系沿水平方向为x方向。

所述的参考坐标系x’y’z’具体是指：以机械臂的关节端面中心为坐标系中心，垂直于关节端面的方向为y’方向，沿竖直方向为z’方向，再根据右手系沿水平方向为x’方向。

所述方法具体为：

2.1)对于每个关节上每根绳的穿绳处，用第n个关节上穿绳处所在的空间点相对于自身关节的参考坐标系{n}的参考坐标位置(xn,yn,zn)，再采用以下公式计算得到穿绳处所在的空间点在绝对坐标系下的绝对坐标位置(x0,y0,z0)：

其中，tn表示机械臂第n个关节的齐次变换矩阵，n表示关节的序数；

2.2)将两个关节间的绳索视为一条直线段，对于同一根绳索，计算相邻两个关节邻接端面的各自穿绳处的空间点之间的距离si-1,i，作为绳索在两个关节间的长度：

其中，xi为第i关节端面上的穿绳处的空间点的绝对坐标的x坐标分量，xi-1为第i-1关节端面上的穿绳处的空间点的绝对坐标的x坐标分量，yi为第i关节端面上的穿绳处的空间点的绝对坐标的y坐标分量，yi-1为第i-1关节端面上的穿绳处的空间点的绝对坐标的y坐标分量，zi为第i关节端面上的穿绳处的空间点的绝对坐标的z坐标分量，zi-1为第i-1关节端面上的穿绳处的空间点的绝对坐标的z坐标分量；

2.3)将所有的关节间绳索长度相加，再加上所有的穿过关节的绳索长度(即关节两个端面的距离l)，就得到了每根绳索的总长，具体采用以下公式计算驱动第n关节的绳索的总长：

其中，si-1,i为第i-1关节和第i关节间的绳索长度，l表示每个关节前后两个端面的距离。

所述的tn表示机械臂第n个关节的齐次变换矩阵具体是采用以下公式计算：

tn＝tn-1×td×rxn×rzn×td×tl

式中，td表示沿y轴平移距离d的平移齐次变换矩阵，d为关节端面到所紧邻连接的万向节中心的距离，rxn表示第n个关节旋转俯仰角θ的旋转齐次变换矩阵，rzn表示第n个关节旋转偏转角的旋转齐次变换矩阵，tl表示沿y轴平移距离l的平移齐次变换矩阵，l表示每个关节前后两个端面的距离。

所述的平移齐次变换矩阵td计算为：

所述的第n个关节的旋转齐次变换矩阵rxn计算为：

所述的第n个关节的旋转齐次变换矩阵rzn计算为：

沿y轴平移距离l的平移齐次变换矩阵tl为：

式中，θ表示关节绕x轴的相对旋转角度，表示关节绕z轴的相对旋转角度。

所述的平移齐次变换矩阵tl、平移齐次变换矩阵td、旋转齐次变换矩阵rxn和旋转齐次变换矩阵rzn代入齐次变换矩阵tn后获得每个关节的齐次变换矩阵计算公式。例如，机械臂第一个关节的齐次变换矩阵t1计算为：

式中，θ表示关节绕x轴的相对旋转角度，表示关节绕z轴的相对旋转角度。

本发明的有益效果是：

由于机械臂关节的相互耦合作用，在控制前端的某些关节运动时必须要同时考虑末端其他关节被动受到的影响，即使末端关节没有相对前端关节运动(即末端关节的空间姿态角度没有发生变化)，驱动末端关节的绳索的长度也会发生变化，需要进行计算。采用本发明的控制方法能够实现解耦，不需要考虑前后关节的相互作用，分别独立求解出驱动各关节的绳索长度的变化，从而获得绳索的总长，实现对机械臂整体姿态的控制。

目前大多数的控制方法仅针对柔性的连续型机械臂，对于具有一定刚性的串联关节型蛇形机械臂均模仿对连续型机械臂的控制方法，采用类似的运动学模型进行研究。传统的连续型机械臂采用弯曲角度和旋转角度来建立运动学模型描述关节的空间姿态，该运动学模型是基于一系列假设的前提条件的，包括弯曲的曲率相等，忽略轴向变形等，在实际情况中难以保证条件全部满足，因此必然会导致误差。而本发明的方法不需要满足这些前提条件，直接精确地描述了关节的空间姿态，因此具有更高的精度。

采用弯曲角度和旋转角度来描述空间姿态有许多弊端。弯曲角度是指旋转后的关节轴线与在关节处于没有任何旋转的初始位置时的关节轴线的夹角，而旋转角度则是表示轴线弯曲朝向的角度，例如一棵树被风吹动时，树的轴线与地面的夹角即为弯曲角度，而树弯曲的朝向可以是0°到360°，这就是旋转角度。采用这两种角度描述空间姿态时并不是一一对应的，两对数值不同的角度却可以描述相同的空间姿态，例如弯曲角度为45°，旋转角度为0°时描述的空间姿态与弯曲角度为135°，旋转角度为180°时描述的空间姿态完全相同，这样会导致对空间姿态进行求解时解不唯一的情况，使问题变得复杂。

此外采用这两种角度描述空间姿态时，在机械臂连续运动的过程中可能会出现角度值突变的情况，例如在弯曲角度为45°，旋转角度为0°时，旋转角度不变的同时弯曲角度连续增加到135°，这样由于前述的多对角度对应同一空间姿态的情况，弯曲角度增加到90°后的瞬间，旋转角度可能由0°突变为180°，这样的不连续性会大大增加控制的难度。

相比之下本发明采用俯仰角度和偏转角度来描述空间姿态不仅可以在角度变化连续的基础上唯一准确地确定空间姿态，而且更加简单直观，符合正常人的认知而便于理解，在空间姿态描述更精确的同时也更易于求解计算。

附图说明

图1为绳驱动串联关节型蛇形机械臂示意图；

图2为绝对坐标系和参考坐标系示意图；

图3为关节旋转俯仰角θ后的姿态示意图；

图4为关节先旋转俯仰角θ再旋转偏转角后的姿态示意图；

图5为机械臂的整体目标空间姿态示意图；

图6为绳索及绳孔位置示意图；

图7为本发明方法流程图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

本发明方法的原理如下：

每个关节的总长为l+2d，其中l为关节前后两个端面的距离，d为关节端面到所紧邻连接的万向节中心的距离。

建立以下基于几何关系的运动学模型：

a)构建绝对坐标系，机械臂的各个关节在初始状态下沿垂直于臂基座的同一直线布置时，第一个万向节的中心位于y轴上，与原点的距离为d，如图2所示。

例如第一关节，以第一关节轴线与y轴重合时为初始位置，构建固连在第一关节上的参考坐标系。设第一关节首先绕x轴旋转了一个角度θ，由于万向节的约束，等效于绕通过第一个万向节中心且平行于x轴的轴线旋转，此时其自身的参考坐标系也绕x轴旋转了相同角度θ而变成如图3所示。

然后在此基础上再绕其自身的变化后的参考坐标系的z轴旋转一个角度，由于万向节的约束，等效于绕通过第一个万向节中心且平行于变化后的z轴的轴线旋转，这样就获得了第一关节在任意转动后的一般姿态，如图4所示。

为了从绝对坐标系变换到第一关节转动后的参考坐标系，首先需要将绝对坐标系先沿y轴平移距离d，然后绕变化后的x轴旋转角度θ，再绕变化后的z轴旋转角度，最后绕变化后的y轴平移距离d+l。

沿y轴平移距离d的平移齐次变换矩阵为：

绕x轴旋转俯仰角θ的旋转齐次变换矩阵为：

绕z轴旋转偏转角的旋转齐次变换矩阵为：

沿y轴平移距离l的平移齐次变换矩阵tl为：

依次进行上述变换，即连续右乘各齐次变换矩阵，获得从绝对坐标系变换到第一关节转动后的参考坐标系的齐次变换矩阵

其中包含了第一关节的转动角度θ和两个变量。

对于从绝对坐标系变换到第二关节的参考坐标系，只需要在t1变换的基础上将变量改为第二关节的转动角度θ2和此时旋转齐次变换矩阵变为rx2和rz2再重复一样的变换步骤即可得到：

t2＝t1×td×rx2×rz2×td×tl

改记第一关节的转动角度为θ1和则t2中包含了θ1、θ2和四个变量。

以此类推，可以获得机械臂各关节的齐次变换矩阵t1、t2、…、tn：

tn＝tn-1×td×rxn×rzn×td×tl

其中包含了θ1、θ2、…、θn、共2n个变量。

在获得各关节的齐次变换矩阵后，就可以计算经过任意角度的转动后关节上穿绳处所在空间点的绝对坐标，空间点相对第n关节参考坐标系{n}的坐标为(xn,yn,zn)，则空间点在绝对坐标系下的绝对坐标(x0,y0,z0)可由下式求出：

在获得空间点的绝对坐标后，就可以计算两点间的距离，作为si-1,i。

每一根绳索都会固定到它所驱动的关节的端面上，并且穿过前一个关节的端面上相应的绳孔，在两个关节间的绳索可以视为一条直线段，直线段的两个端点分别位于前后两个关节的端面上已知的确定位置，因此可以通过求出端点的绝对坐标来计算两个端点的距离，从而获得绳索在两个关节间的长度。

在求出每根绳索在每两个关节间的长度后，将所有的关节间绳索长度相加，再加上所有的穿过关节的绳索长度(即关节两个端面的距离l)，就得到了每根绳索的总长。对于驱动第n关节的绳索，其总长为：

其中si-1,i为第i-1关节和第i关节间的绳索长度，当i＝1时si-1,i表示基座端面和第一关节间的绳索长度。由于最后一个关节中没有绳索穿过，驱动该关节的绳索仅固定到该关节的前端面，因此只加上其余关节中穿过的绳索长度(n-1)×l。由于每个关节至少由三根绳索驱动，根据不同的绳孔位置，至少有三个ln，且值不一定相同。

设每个关节由m根绳索驱动，则总共需要求出m×n个值。在求出所有绳索各自对应的绳索总长l1、l2、…、lm×n后，只需要控制相应的绳索拉到相应的总长，就可以控制机械臂运动到由变量θ1、θ2、…、θn、所确定的整体姿态，实现对机械臂空间姿态角度的控制。

本发明的实施例如下：

以总计6个关节，每个关节由3根绳索驱动为例，机械臂每个关节都有18个绳孔，绳孔位于每个关节的两个端面上，以关节轴线与端面的交点为圆心，等角度均布于半径r＝0.042m的圆周上，如图6所示。机械臂每个关节两个端面的距离l＝0.147m，关节端面到万向节中心的距离d＝0.019m。

并设机械臂需要运动到θ1＝30°，θ2＝-30°，θ3＝-30°，θ4＝30°，θ5＝30°，θ6＝-30°，所有关节角度均为0°的姿态，如图5所示。

由已知量获得各齐次变换矩阵：

例如对于第一关节有：

求出从绝对坐标系变换到第一关节转动后的参考坐标系的齐次变换矩阵：

然后求出从绝对坐标系变换到第二关节转动后的参考坐标系的齐次变换矩阵：

以此类推，依次求出t3、t4、t5、t6。

如图6所示，然后计算绳索端点的绝对坐标，以靠近基座的端面为前端面，绳索一端与驱动机构固定，另一端从前一个关节的后端面穿出，与绳索驱动的关节的前端面固定。驱动第一关节的绳索从基座端面穿出，固定到第一关节的前端面上；驱动第二关节的绳索穿过基座后再穿过第一关节，从第一关节的后端面穿出，固定到第二关节的前端面上，以此类推。

从第一关节与机械臂基座间的绳索开始。对于第一根绳索，它在基座端面上的端点的绝对坐标为(0,0,r)，即(0,0,0.042)，由于基座的参考坐标系就是绝对坐标系，不需要再转换；第一根绳索在第一关节前端面上的参考坐标系的坐标为(0,-l,r)，即(0,-0.147,0.042)，则其绝对坐标(x0,y0,z0)为：

获得两端点的绝对坐标后就可以求出第一根绳索在基座与第一关节间的长度，然后以此类推，求出所有绳索在基座与第一关节间的长度，再以此类推，向后方递进，逐级求出各关节间的绳索长度。最后对每一根绳索，将各关节间的长度再加上穿过关节的绳索长度，就获得了单根绳索的总长。列出求出的18根绳索相应的总长(单位为m)如下：

l1＝0.0150，l2＝0.2204，l3＝0.4208，l4＝0.5878，l5＝0.7678，l6＝0.9552，l7＝0.0476，l8＝0.2204，l9＝0.3837，l10＝0.5878，l11＝0.7920，l12＝0.9552，l13＝0.0476，l14＝0.2204，l15＝0.4079，l16＝0.5878，l17＝0.7549，l18＝0.9552。

驱动机构拉动绳索，使绳索穿出基座端面的长度分别为以上相应的值，即可控制机械臂运动到给定的姿态。

由以上实施例可见，本发明能够精确唯一地描述机械臂的整体空间姿态，实现解耦，不需要解方程组就能够分别独立求解出驱动各关节的绳索长度的变化，运算量小，效率高，实时性好。