[发明专利]一种绳驱动串联关节型蛇形机械臂控制方法

权利要求书

1.一种绳驱动串联关节型蛇形机械臂控制方法，所述的机械臂是由绳索驱动的机械臂，机械臂由多个关节串联而成，关节之间使用万向节连接，绳索连接到各个关节，其特征在于方法包括：

1)先构建绝对坐标系XYZ和每个关节的参考坐标系X’Y’Z’；

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

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

2.根据权利要求1所述的一种绳驱动串联关节型蛇形机械臂控制方法，其特征在于：所述的绝对坐标系XYZ具体是指：以机械臂的臂基座端面中心为坐标系中心，沿垂直于臂基座端面的方向为Y方向，沿竖直方向为Z方向，再根据右手系沿水平方向为X方向。

3.根据权利要求1所述的一种绳驱动串联关节型蛇形机械臂控制方法，其特征在于：所述的参考坐标系X’Y’Z’具体是指：以机械臂的关节端面中心为坐标系中心，垂直于关节端面的方向为Y’方向，沿竖直方向为Z’方向，再根据右手系沿水平方向为X’方向。

4.根据权利要求1所述的一种绳驱动串联关节型蛇形机械臂控制方法，其特征在于：所述方法具体为：

2.1)对于每个关节上每根绳的穿绳处，用第n个关节上穿绳处所在的空间点相对于自身关节的参考坐标系{n}的参考坐标位置(x_n,y_n,z_n)，再采用以下公式计算得到穿绳处所在的空间点在绝对坐标系下的绝对坐标位置(x₀,y₀,z₀)：

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

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

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

2.3)采用以下公式计算驱动第n关节的绳索的总长：

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

5.根据权利要求4所述的一种绳驱动串联关节型蛇形机械臂控制方法，其特征在于：所述的T_n表示机械臂第n个关节的齐次变换矩阵具体是采用以下公式计算：

T_n＝T_n-1×T_d×R_xn×R_zn×T_d×T_L

式中，T_d表示沿Y轴平移距离d的平移齐次变换矩阵，d为关节端面到所紧邻连接的万向节中心的距离，R_xn表示第n个关节绕X轴的相对旋转角度θ的旋转齐次变换矩阵，R_zn表示第n个关节旋转偏转角的旋转齐次变换矩阵，T_L表示沿Y轴平移距离L的平移齐次变换矩阵，L表示每个关节前后两个端面的距离。

6.根据权利要求5所述的一种绳驱动串联关节型蛇形机械臂控制方法，其特征在于：所述的平移齐次变换矩阵T_d计算为：

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

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

所述的平移齐次变换矩阵T_L为：

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