CN110076775A - 一种绳驱动连续型机械臂的三维静力学建模方法 - Google Patents

Abstract

本发明公开了一种绳驱动连续型机械臂的三维静力学建模方法，包括以下步骤：S1：根据三关节伪刚体模型，建立每个等效的扭簧的三维力平衡方程，得到所述绳驱动连续型机械臂受到预定的绳索拉力、外力和重力时的每个所述扭簧的偏转角度；S2：根据每个所述扭簧的偏转角度，分别建立绳索拉力、外力和重力对每个所述分节末端的合力和合力矩表达式；S3：将绳索拉力、外力和重力对每个所述分节末端的合力和合力矩表达式代入到每个所述扭簧的三维力平衡方程，得到所述绳驱动连续型机械臂的三维静力学平衡方程。本发明将二维平面的伪刚体等效方法拓展到三维空间中，且大大提高运算效率。

Description

一种绳驱动连续型机械臂的三维静力学建模方法

技术领域

本发明涉及机械臂建模技术领域，尤其涉及一种绳驱动连续型机械臂的三维静力学建模方法。

背景技术

连续型柔性机械臂受自然界中象鼻等生物体结构启发，一般由弹性物体作为支撑，将许多模块化关节串联而成，或者直接用完整无间断的弹性材料作为机械臂本体，因而具有超高冗余度甚至理论上无限多自由度。这种结构形式使得连续型机械臂具有良好的运动灵活性和柔顺性，因而特别适合于狭小空间下的避障作业。连续型空间机械臂可以穿越航天器的桁架结构和组件间隙，深入到结构内部进行探测、维修等任务。因此，连续型柔性机械臂的灵活性、柔顺性和细长特点使得其在空间狭小复杂环境中具有广阔的应用前景。

目前连续型机械臂的运动学基本上是基于常曲率的假设所进行的理论推导，但是实际的模型往往不是常曲率的。为了获取更精确地建模，不得不考虑外力、外力矩、摩擦力、自身的弹力、重力等的作用。关于连续型柔性机械臂的静力学分析方法目前主要有四种理论分支：1)基于Cosserat rod theory(Cosserat杆理论)的分析方法，主要用在弹性杆作为脊柱线或者气/液驱动这些类型，并且是变曲率的情况下；2)基于Kirchhoffrodtheory(基尔霍夫杆理论)的分析方法，这种主要用在同心管机械臂上；3)基于欧拉梁模型的分析方法，主要用在同心管机械臂上，当然也可以用于分析简单的平面弯曲效果；Gravagne采用欧拉梁模型对机械臂进行建模，但是欧拉梁模型的求解含有积分项，运算效率较低，并且不能很好地提取其中的影响参数进行具体的分析；4)基于常曲率的虚功原理，它主要用在腱驱动类型，重点用于讨论摩擦力以及驱动力的控制方法。学者们在各个方面对绳索驱动连续型机械臂进行更加细致的研究。在外力的负载分析上，Rucker利用虚功原理对存在外负载的情况进行建模分析，但是没有考虑摩擦力的影响。Rone探讨了多段绳索驱动连续型机械臂的摩擦力影响，同时也讨论了两段之间会有耦合影响，但是没有将负载添加进去考虑，也没有定性分析造成耦合影响的具体因素。由于机械臂同一个静止状态可能有着不同的静摩擦力，该摩擦力由于运动过程的不同以及机械臂当前状态的不同会有着不同的静摩擦系数。并且静摩擦力的分析涉及到微观结构，目前的学者都难以在绳索驱动的连续型机械臂上定性讨论平衡状态的静摩擦力的影响。也有部分学者对该影响因素进行定量的分析，通过数值上的表现来考虑该影响因素。而机械臂的运动误差是多方面的，摩擦力会导致末端位置的不稳定，不同小节的误差也有不一样的表现。前面的学者并没有将不同小节的误差以及摩擦力的影响整合到一起进行分析。

另一方面，在连续体的静力学建模上许多学者会采用伪刚体的方法对梁模型进行等效，通过该方法可以极大地简化该模型的复杂度，在实际应用当中有着重要的意义。Khoshnam通过伪刚体模型的等效，建立了同心管型连续型机械臂的静力学模型，并且将其应用到实际上；但是Khoshnam所采用的模型只能用在二维的平面空间内进行计算，无法适用三维的场景。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

为解决上述技术问题，本发明提出一种绳驱动连续型机械臂的三维静力学建模方法，将二维平面的伪刚体等效方法拓展到三维空间中，且大大提高运算效率。

为了达到上述目的，本发明采用以下技术方案：

本发明公开了一种绳驱动连续型机械臂的三维静力学建模方法，包括以下步骤：

S1：根据三关节伪刚体模型，建立每个等效的扭簧的三维力平衡方程，得到所述绳驱动连续型机械臂受到预定的绳索拉力、外力和重力时的每个所述扭簧的偏转角度；其中所述绳驱动连续型机械臂包括多个分段，每个分段分别包括多个分节，每个分节之间依次通过圆盘连接起来，且每相邻的两个所述圆盘之间夹着一个弹性部件，每个分段分别对应多根驱动绳索，且所有驱动绳索分别穿过多个所述圆盘上至所述绳驱动连续型机械臂的一端以通过所有驱动绳索施加绳索拉力来驱动所述弹性部件弯曲；每个所述弹性部件等效为由三个扭簧连接的四段刚性杆组成的梁；

S2：根据每个所述扭簧的偏转角度，分别建立绳索拉力、外力和重力对每个所述分节末端的合力和合力矩表达式；

S3：将绳索拉力、外力和重力对每个所述分节末端的合力和合力矩表达式代入到每个所述扭簧的三维力平衡方程，得到所述绳驱动连续型机械臂的三维静力学平衡方程。

优选地，步骤S1中根据三关节伪刚体模型，建立每个等效的扭簧的三维力平衡方程为：

其中，i表示整个机械臂的分节的编号，r表示某一分节内扭簧的编号，其中r＝1,2,3；τ_i,r表示第i分节内第r个扭簧对应的扭力，K_r表示伪刚体中的第r个扭簧的等效刚度系数，θ_i,r表示第i分节内第r个扭簧的偏转角度，表示绳索拉力对第i分节内第r个扭簧作用的等效力矩，表示外力对第i分节内第r个扭簧作用的等效力矩，表示重力对第i分节内第r个扭簧作用的等效力矩。

优选地，步骤S2中根据每个所述扭簧的偏转角度，建立绳索拉力对每个所述分节末端的合力F_i ^C和合力矩表达式分别为：

其中，i表示整个机械臂的分节的编号，k表示驱动绳索对应的绳孔的编号，j表示驱动绳索所属的分段的编号，n表示每一分段中分节的数量，m表示每一分段对应的驱动绳索的数量，s表示所述绳驱动连续型机械臂包含的分段的数量；^kF_i,j表示第i分节的第k根绳索的拉力，O_i+1表示第i+1分节的原点，^kA_i+1,j表示第i+1分节的第k根绳索对应的绳孔的位置。

优选地，其中第i分节的第k根绳索的拉力^kF_i,j的计算公式为：

^kF_i,j＝^kf_i,j*^kp_i,j

其中，^kf_i,j表示第i分节的第k根绳索的拉力大小，^kp_i,j表示第i分节的第k根绳索的拉力方向；

且第i分节的第k根绳索的拉力大小^kf_i,j的递推计算公式为：

其中，μ表示绳索与绳孔之间的摩擦系数，^kq_i,j表示第i分节的第k根绳索的摩擦力的方向，^kη_i+1,j表示^kp_i,j与^kp_i+1,j之间的夹角，^kf_0,j为初始的驱动绳索拉力大小；

第i分节的第k根绳索的拉力方向的计算公式为：

^kp_i,j＝(^kA_i,j-^kA_i+1,j)/||^kA_i,j-^kA_i+1,j||

其中，^kA_i,j表示第i分节的第k根绳索对应的绳孔的位置。

优选地，第i+1分节的第k根绳索对应的绳孔的位置^kA_i+1,j的递推计算公式为：

^kA_i+1,j＝ⁱT_i+1 ^kA_i,j

其中，ⁱT_i+1表示从O_i坐标系变换到O_i+1坐标系的坐标变换矩阵。

优选地，从O_i坐标系变换到O_i+1坐标系的坐标变换矩阵ⁱT_i+1根据三关节伪刚体模型来建立：

其中，表示沿着z轴旋转的旋转矩阵，表达式为：

其中，表示第i分节的旋转角度；

T_i,a表示第i分节第a段刚体杆的变换矩阵，其表达式为：

其中，a表示每一分节内的刚性杆的编号，a＝0,1,2,3，θ_i,0＝0，θ_i,1、θ_i,2、θ_i,3分别为第i分节内第1、2、3个扭簧的偏转角度，γ_a表示第a段刚体杆的比例系数。

优选地，其中第1分节的第k根绳索对应的绳孔的位置^kA_1,j的计算公式为：

^kA_1,j＝[d cos(α+β+ψ) d sin(α+β+ψ) 0]

其中，β＝2π(j-1)/(m*s)，α＝2π(k-1)/m，d是绳孔中心到圆盘中心的距离，ψ表示驱动第一分段的第一根驱动绳索的偏转角度。

优选地，步骤S2中根据每个所述扭簧的偏转角度，建立外力对每个所述分节末端的合力F_i ^e和合力矩表达式分别为：

F_i ^e＝F^e

其中，F^e表示外力，E为外力F^e的作用点，F_i ^e是映射后的外力合力，是映射后的合力矩。

优选地，步骤S2中根据每个所述扭簧的偏转角度，建立重力对每个所述分节末端的合力F_i ^g和合力矩表达式分别为：

其中，G_b表示第b分节末端受到的重力。

优选地，步骤S3得到的所述绳驱动连续型机械臂的三维静力学平衡方程包括：

<n_i,F_i ^Total>＝π/2

其中，n_i表示第i分节旋转后所在平面的法向量，K_r表示伪刚体中的第r个扭簧的等效刚度系数，θ_i,r表示第i分节内第r个扭簧的偏转角度，P_i,r表示第i分节内第r个扭簧的位置，F_i ^Total和分别表示所有外力在第i分节末端的合力和合力矩，表达式分别为：

F_i ^Total＝F_i ^C+F_i ^e+F_i ^g

与现有技术相比，本发明的有益效果在于：本发明公开的绳驱动连续型机械臂的三维静力学建模方法根据以弹性部件为轴心的连续型机械臂的特点，结合三关节伪刚体模型建立三维的多节连续型机械臂的静力学模型，以将连续型机械臂转化成传统的离散型机械臂结构进行分析，解决了传统梁理论力学模型因为具有积分项从而导致计算效率低的问题；该模型同时考虑了摩擦力、重力、外力以及机械臂自身弹性力的影响，具有很高的精度，而且可以被高效求解，为多段连续型机械臂的设计提供重要参考意义。

附图说明

图1是本发明优选实施例的绳驱动连续型机械臂的几何结构图；

图2a是图1中的连续型机械臂的一个分节的参数定义图；

图2b是图1中的连续型机械臂的一个分节的三维伪刚体等效结构图；

图2c是图1中的连续型机械臂的一个分节的偏转方法定义以及偏转平面的法向量定义图；

图3是采用本发明优选实施例的方法计算的末端位置与实验实际测量的对比图；

图4是采用本发明优选实施例的方法计算的偏转角度与试验实际测量的对比图。

具体实施方式

下面对照附图并结合优选的实施方式对本发明作进一步说明。

Greigarn建立了广义的伪刚体模型，并且将其应用于连续型机械臂之上，但是因为伪刚体参数过于复杂，需要引入新的优化算法才能得到较好的预测结果。尽管有了之前这些研究者的尝试，但是伪刚体模型在连续型机械臂上的应用还是很少。一方面是PRB 3R模型在本发明之前只能被用于二维平面上的计算；另一方面，PRB的广义形式又涉及到参数的优化问题，在简化连续型机械臂的模型上面仍然有着很大的挑战。现如今的绝大多数连续型机械臂都工作在三维的空间上，因此很有必要将PRB 3R模型推广到三维的空间中进行建模。同时，在静力学方面的建模也是现如今连续型机械臂的重要方面。

鉴于此，本发明优选实施例公开了一种绳驱动连续型机械臂的三维静力学建模方法，下述以图1所示的绳驱动连续性机械臂为例对该连续型机械臂的静力学建模方法加以说明。

1、几何结构的分析

如图1所示，椭圆表示的是每一个支撑的圆盘(Disk)10，中轴线表示机械臂中间的弹性部件20，下方的细线表示的是每一分段对应的驱动绳索(Cable)30，该绳驱动连续型机械臂包括s分段(Seg)(也即该绳驱动连续型机械臂的分段数为s)，其中每分段包含n个分节(也即每分段内包含的分节数为n)，每一分段由m根驱动绳索(Cable)来驱动(也即每分段内包含的驱动绳索的数量为m)，绳孔均匀地分布在圆盘上的同一个半径的圆上。如图1中所示，第一分段(Seg1)中包含圆盘Disk1～Disk n+1，对应的驱动绳Cable c，其中c＝1,2,……,m；第二分段(Seg2)包含圆盘Diskn+1～Disk 2n+1，对应的驱动绳Cable c，其中c＝m+1,m+2,……,2m；……第s分段(Seg s)包含圆盘Disk(s-1)*n+1～Disk s*n+1，对应的驱动绳Cable c，其中c＝(s-1)*m+1,(s-1)*m+2,……,s*m；也可以看出控制第j分段的驱动绳索的末端将会被固定在第j*n+1的圆盘上，因此传递到下一分段的绳索数量将会减少m根。其中定义每两个圆盘(Disk)10之间的部件(包含这两个圆盘10)为一个分节。

在本发明优选实施例中，基于以下两个假设对该绳驱动连续型机械臂进行建模：(1)由于沿着轴向的刚度要远大于弯曲的刚度，因此在运动过程中，每一个分节中间的轴线的长度始终保持为L；(2)中轴线20不会发生扭转，只会沿着某一个平面发生弯曲，并且弯曲的变形满足线性的弹性形变关系。

2、PRB 3R模型

对于简单的变形(恒定的外力作用或者恒定的力矩作用在梁的末端)，采用PRB 1R(单关节伪刚体)的模型足以进行分析，但是绳索驱动的连续型机械臂的驱动力是一个变力，方向和大小都会不断改变。因此在本实施例中，采用PRB3R(三关节伪刚体)模型来对该绳驱动连续型机械臂进行分析，通过该伪刚体模型可以做到即使在大变形的范围也能将误差控制在1％以内。

该方法的实质在于将一根杆长为l的梁，等效成四段刚性杆，中间用三个扭簧连接而成，关键在于求解关于这三个扭簧的力平衡方程如式(1)所示：

其中，τ₁、τ₂、τ₃分别是三个扭簧对应的扭力，J^T是关于三个扭簧的偏转角度以及四段刚性杆等效的长度比例所组成的矩阵，F_x、F_y以及M₀表示外力的作用，具体地，F_x表示梁末端在x方向的受力，F_y表示梁末端在y方向的受力，M₀表示梁末端所受的力矩，l表示杆的初始长度。

在本实施例中，要建立在三维运动情况下的伪刚体模型，其中图2b所示的是一个分节在三维空间下的伪刚体等效模型，在三维的情况下，不仅需要考虑每个关节的偏转角度，还就需要考虑图2c所示的第i分节的旋转角度的求解问题。

对于绳驱动连续型机械臂，可以将所有受到的外力(绳索拉力、外部作用力和重力)都转化为力矩，得到每一个等效的扭簧的力平衡方程为：

其中，i表示分节的编号，r＝1,2,3表示某一分节内扭簧的编号；τ_i,r表示第i分节内第r个扭簧对应的扭力，K_r表示伪刚体中的第r个扭簧的等效刚度系数，θ_i,r表示第i分节内第r个扭簧的偏转角度，表示绳索拉力对第i分节内第r个扭簧作用的等效力矩，表示外力对第i分节内第k个扭簧作用的等效力矩，表示重力对第i分节内第k个扭簧作用的等效力矩。

3、运动学

图2a示出了第i分节上面的绳孔分布情况以及圆盘的几何结构，其中^kA_i,j表示第i分节的第k根绳索对应的绳孔的位置(也即表示绳孔的位置)，k表示绳孔的编号，j表示该绳孔所属的分段，i表示整个机械臂的分节的编号；图中列举了经典的三个绳孔的情况，其他绳孔数量以此类推即可；ψ表示驱动第一分段的第一根驱动绳索于世界坐标系的x_G轴在x_GO_Gy_G平面上的偏转角度，该偏转角度ψ是一个常数，由机械结构和坐标系的定义方式来决定；O_i表示的是第i分节对应坐标系的中心，坐标系的定义方式如图所示，其中基座Disk 1的坐标系与世界坐标系[O_G,x_G,y_G,z_G]重合，图2b中的P_i,r表示第i分节内第r个扭簧的位置(也即三个伪刚体的关节位置)，θ_i,r表示第i分节内第r个扭簧的偏转角度(也即三个关节的偏转角)，γ_a(a＝0,1,2,3，a表示某一分节内伪刚体系数编号)表示伪刚体的比例系数，L表示一个分节的杆长，图2c中的表示第i分节的旋转角度，n_i表示第i分节旋转后所在平面的法向量。

根据伪刚体等效模型，建立从O_i坐标系变换到O_i+1坐标系的坐标变换矩阵ⁱT_i+1为：

其中，表示沿着z轴旋转的旋转矩阵，表达式为：

如图2b所示，T_i,a(a＝0,1,2,3)表示第i分节第a段刚体杆的变换矩阵(也即沿着四段刚性杆的方向平移旋转矩阵)，其表达式为：

其中，θ_i,0＝0，θ_i,1、θ_i,2、θ_i,3分别为第i分节内第1、2、3个扭簧的偏转角度。

具体地，T_i,0表示经过旋转后从O_i变换到P_i,1的变换矩阵，T_i,1表示从P_i,1变换到P_i,2的变换矩阵，T_i,2表示从P_i,2变换到P_i,3的变换矩阵，T_i,3表示从P_i,3变换到还未旋转-前的O_i+1的变换矩阵，O_i表示第i分节的原点，P_i,r表示第i分节第r个扭簧的位置点。

根据图2a的几何结构，可以得到Disk 1上的过绳孔的坐标的通用表达式如下所示：

^kA_1,j＝[d cos(α+β+ψ) d sin(α+β+ψ) 0] (6)

其中，β＝2π(j-1)/(m*s)，α＝2π(k-1)/m，d是绳孔中心到圆盘中心的距离；再通过坐标变换矩阵，可以根据基座的坐标推导出后面所有圆盘上面的过绳孔的坐标表达式为：

^kA_i+1,j＝ⁱT_i+1 ^kA_i,j (7)

其中，ⁱT_i+1表示从O_i坐标系变换到O_i+1坐标系的坐标变换矩阵。

4、静力学

在本实施例中，通过分析其中第i分节的静力学模型便可以递推出整个机械臂的静力学模型。如图2a所示，拉力的方向可以用两个圆盘之间的绳孔连线来表示：由第i+1个圆盘的绳孔指向第i个圆盘上与之对应的绳孔，因此绳孔拉力的方向^kp_i,j为：

^kp_i,j＝(^kA_i,j-^kA_i+1,j)/||^kA_i,j-^kA_i+1,j|| (8)

进一步，绳索拉力可以表示为：

^kF_i,j＝^kf_i,j*^kp_i,j (9)

其中，^kf_i,j表示绳索拉力的大小。

考虑了绳孔之间的摩擦力之后，如果绳索所在的分节发生弯曲，它的绳索拉力相对于前一分节将会发生改变，其大小关系可以从根部末端进行递推，递推公式如下所示：

其中，^kq_i,j表示第i分节的第k根绳索对应的摩擦力的方向，取值为1或-1，具体为何值，取决于机械臂的运动历史；μ表示绳索与绳孔之间的摩擦系数，取决于材料特性；^kη_i+1,j表示^kp_i,j与^kp_i+1,j之间的夹角；其中特别地，i＝0的时候，^kf_0,j为初始的驱动绳索拉力大小。

为了得到最后的平衡方程，首先将所有的力映射到每一个分节的末端圆盘的中心。第一分段的每个圆盘会受到驱动所有分段的绳索拉力的作用，第二分段将不再受到第一分段的绳索拉力的作用，以此类推。

首先，根据上述绳索的作用关系，得到在驱动绳索拉力作用下，第i分节的末端圆盘中心受到的绳索拉力的合力F_i ^C和合力矩的公式为：

其次，假设外力F^e作用在E点，E点的坐标可以采用运动学关系得出。因此，外力在第i分节末端的映射关系如下所示：

F_i ^e＝F^e (13)

其中，公式(13)表示力的映射关系，F_i ^e是映射后的合力，公式(14)表示力矩的映射关系，是映射后的合力矩。

再次，假设第i+1个圆盘可以等效为一个位于O_i+1上的质点，可以得到第b分节末端受到的重力G_b作用如下所示：

最后，得到所有外力在第i分节末端的合力F_i ^Total以及合力矩如下所示：

F_i ^Total＝F_i ^C+F_i ^e+F_i ^g (17)

另一方面，需要讨论偏转角度的计算。由之前的假设，已经确定在平衡状态下，该分节的脊柱线会在一个平面上，因此，为了满足该平衡条件，和F_i ^Total的方向需要满足以下两个条件：1)垂直于偏转平面，2)F_i ^Total平行于偏转平面。因此得到如下两个约束关系式：

<n_i,F_i ^Total>＝π/2 (20)

最后将公式(17)和(18)代入到公式(2)中，得到力矩平衡方程的通式为：

通过Matlab的fsolve函数对方程(19)、(20)和(21)进行求解即得到给定外力作用下机械臂的每个分节的旋转角度以及每个分节内每个扭簧对应的偏转角度θ_i,r。

相对于传统的欧拉梁模型，基于伪刚体的建模方法可以将连续型机械臂等效成传统的刚性机械臂；基于此方法建立的静力学方程不含有积分项，可以用传统的刚性机械臂理论进行分析，因此方程很容易被求解；在实时控制上也会起到很好的效果。因此本发明公开的基于伪刚体的三维连续型机械臂静力学建模方法能够应用于连续型机械臂模型简化、静力学分析、动力学分析等方面，具备以下功能：(1)具备将连续型机械臂等效为传统刚性臂的功能；(2)具备分析绳驱动连续型机械臂的摩擦力、重力、外力作用的功能；(3)具备采用伪刚体的方法建立三维情况下的静力学方程的功能。

本发明的方法还具有以下优点：(1)通过伪刚体的等效方法，将连续型机械臂等效成传统刚性臂进行分析，大大降低分析难度；(2)通过伪刚体的等效方法建立的静力学方程有着极高的运算效率；(3)在该静力学方程的基础上建立了同时考虑绳驱动连续型机械臂的摩擦力以及除驱动力以外的外力负载还有重力作用的静力学方程；(4)将原来的二维平面上的静力学方程拓展到三维空间中；(5)将二维平面的伪刚体等效方法拓展到三维空间中。

下述通过具体实验来对本发明优选实施例提出的绳驱动连续型机械臂的三维静力学建模方法进行验证。设计一个分节的模型，由三根绳索来驱动其运动，通过验证其弯曲平面的方向角以及末端的坐标，来验证本发明优选实施例的模型的准确性。将一个分节固定在桌面上，末端放置一个靶球，采用三维相机(optictrack-)采集末端靶球的坐标；通过对三根绳索(参数)悬挂不同的重物来获取不同的静力平衡状态。通过三维相机可以采集到该小节在平衡状态下的靶球坐标，在实验之前对相机系统进行标定，建立以基座为中心的世界坐标系，相机的精度可以达到0.2mm；本实验的绳孔与驱动绳索的摩擦系数通过参考文献的方法进行测定得到μ＝0.12。实验参数如表1所示。

表1实验参数

符号	参数	数值
			E	杨氏模量	6.79·10<sup>10</sup>Pa
I	转动惯量	4.83·10<sup>-12</sup>m<sup>4</sup>
			d	过孔半径	0.04m
g	重力加速度	9.785m/s<sup>2</sup>
			L	弹性杆长度	0.25m
m<sub>d</sub>	圆盘质量	4.79·10<sup>-2</sup>kg

伪刚体参数采用如表2所示的参数：

表2伪刚体参数

将通过上述实验的结果与通过本发明优选实施例的模型的计算结果进行对比，图3所示为本发明优选实施例的方法计算的末端位置与实验实际测量的对比图，其中最大的位置误差为7.6％，图4所示为采用本发明优选实施例的方法计算的偏转角度与实验实际测量的对比图，其中最大角度偏差为4.6％；通过对比，可以看出本发明优选实施例的模型可以得到较好的预测效果。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (10)

1.一种绳驱动连续型机械臂的三维静力学建模方法，其特征在于，包括以下步骤：

S1：根据三关节伪刚体模型，建立每个等效的扭簧的三维力平衡方程，得到所述绳驱动连续型机械臂受到预定的绳索拉力、外力和重力时的每个所述扭簧的偏转角度；其中所述绳驱动连续型机械臂包括多个分段，每个分段分别包括多个分节，每个分节之间依次通过圆盘连接起来，且每相邻的两个所述圆盘之间夹着一个弹性部件，每个分段分别对应多根驱动绳索，且所有驱动绳索分别穿过多个所述圆盘上至所述绳驱动连续型机械臂的一端以通过所有驱动绳索施加绳索拉力来驱动所述弹性部件弯曲；每个所述弹性部件等效为由三个扭簧连接的四段刚性杆组成的梁；

S2：根据每个所述扭簧的偏转角度，分别建立绳索拉力、外力和重力对每个所述分节末端的合力和合力矩表达式；

S3：将绳索拉力、外力和重力对每个所述分节末端的合力和合力矩表达式代入到每个所述扭簧的三维力平衡方程，得到所述绳驱动连续型机械臂的三维静力学平衡方程。

2.根据权利要求1所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，步骤S1中根据三关节伪刚体模型，建立每个等效的扭簧的三维力平衡方程为：

其中，i表示整个机械臂的分节的编号，r表示某一分节内扭簧的编号，其中r＝1,2,3；τ_i,r表示第i分节内第r个扭簧对应的扭力，K_r表示伪刚体中的第r个扭簧的等效刚度系数，θ_i,r表示第i分节内第r个扭簧的偏转角度，表示绳索拉力对第i分节内第r个扭簧作用的等效力矩，表示外力对第i分节内第r个扭簧作用的等效力矩，表示重力对第i分节内第r个扭簧作用的等效力矩。

3.根据权利要求1所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，步骤S2中根据每个所述扭簧的偏转角度，建立绳索拉力对每个所述分节末端的合力F_i ^C和合力矩表达式分别为：

其中，i表示整个机械臂的分节的编号，k表示驱动绳索对应的绳孔的编号，j表示驱动绳索所属的分段的编号，n表示每一分段中分节的数量，m表示每一分段对应的驱动绳索的数量，s表示所述绳驱动连续型机械臂包含的分段的数量；^kF_i,j表示第i分节的第k根绳索的拉力，O_i+1表示第i+1分节的原点，^kA_i+1,j表示第i+1分节的第k根绳索对应的绳孔的位置。

4.根据权利要求3所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，其中第i分节的第k根绳索的拉力^kF_i,j的计算公式为：

^kF_i,j＝^kf_i,j*^kp_i,j

其中，^kf_i,j表示第i分节的第k根绳索的拉力大小，^kp_i,j表示第i分节的第k根绳索的拉力方向；

且第i分节的第k根绳索的拉力大小^kf_i,j的递推计算公式为：

其中，μ表示绳索与绳孔之间的摩擦系数，^kq_i,j表示第i分节的第k根绳索的摩擦力的方向，^kη_i+1,j表示^kp_i,j与^kp_i+1,j之间的夹角，^kf_0,j为初始的驱动绳索拉力大小；

第i分节的第k根绳索的拉力方向的计算公式为：

^kp_i,j＝(^kA_i,j-^kA_i+1,j)/||^kA_i,j-^kA_i+1,j||

其中，^kA_i,j表示第i分节的第k根绳索对应的绳孔的位置。

5.根据权利要求4所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，第i+1分节的第k根绳索对应的绳孔的位置^kA_i+1,j的递推计算公式为：

^kA_i+1,j＝ⁱT_i+1 ^kA_i,j

其中，ⁱT_i+1表示从O_i坐标系变换到O_i+1坐标系的坐标变换矩阵。

6.根据权利要求5所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，从O_i坐标系变换到O_i+1坐标系的坐标变换矩阵ⁱT_i+1根据三关节伪刚体模型来建立：

其中，表示沿着z轴旋转的旋转矩阵，表达式为：

其中，表示第i分节的旋转角度；

T_i,a表示第i分节第a段刚体杆的变换矩阵，其表达式为：

其中，a表示每一分节内的刚性杆的编号，a＝0,1,2,3，θ_i,0＝0，θ_i,1、θ_i,2、θ_i,3分别为第i分节内第1、2、3个扭簧的偏转角度，γ_a表示第a段刚体杆的比例系数。

7.根据权利要求5所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，其中第1分节的第k根绳索对应的绳孔的位置^kA_1,j的计算公式为：

^kA_1,j＝[dcos(α+β+ψ) dsin(α+β+ψ) 0]

其中，β＝2π(j-1)/(m*s)，α＝2π(k-1)/m，d是绳孔中心到圆盘中心的距离，ψ表示驱动第一分段的第一根驱动绳索的偏转角度。

8.根据权利要求3所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，步骤S2中根据每个所述扭簧的偏转角度，建立外力对每个所述分节末端的合力F_i ^e和合力矩表达式分别为：

F_i ^e＝F^e

其中，F^e表示外力，E为外力F^e的作用点，F_i ^e是映射后的外力合力，是映射后的合力矩。

9.根据权利要求8所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，步骤S2中根据每个所述扭簧的偏转角度，建立重力对每个所述分节末端的合力F_i ^g和合力矩表达式分别为：

其中，G_b表示第b分节末端受到的重力。

10.根据权利要求9所述的绳驱动连续型机械臂的三维静力学建模方法，其特征在于，步骤S3得到的所述绳驱动连续型机械臂的三维静力学平衡方程包括：

<n_i,F_i ^Total>＝π/2

其中，n_i表示第i分节旋转后所在平面的法向量，K_r表示伪刚体中的第r个扭簧的等效刚度系数，θ_i,r表示第i分节内第r个扭簧的偏转角度，P_i,r表示第i分节内第r个扭簧的位置，F_i ^Total和分别表示所有外力在第i分节末端的合力和合力矩，表达式分别为：

F_i ^Total＝F_i ^C+F_i ^e+F_i ^g