CN112917469B - 无外部传感器的绳驱柔性机械臂的末端力感知方法及装置 - Google Patents

Abstract

本发明涉及一种无外部传感器的绳驱柔性机械臂的末端力感知方法及装置。其中的方法包括：建立电机编码器反馈的旋转位置到绳索运动长度的转换关系；建立电机驱动反馈电流到绳索拉力的转换关系；建立机械臂的杆件坐标系；建立末段臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力；建立倒数多段臂杆的动力学平衡方程与力矩平衡方程，迭代计算末端作用力；判断前后两个计算的末端作用力误差是否低于阈值，如果是，则停止迭代。其中的装置包括存储器和处理器，该处理器实施所述方法。通过迭代算机械臂受到的末端力，使绳驱柔性机械臂无需外部传感器的情况下仍然能够实现对末端力信息的准确感知。

Description

无外部传感器的绳驱柔性机械臂的末端力感知方法及装置

技术领域

本发明属于机械臂技术领域，涉及一种无外部传感器的绳驱柔性机械臂的末端力感知方法及装置。

背景技术

当前，绳驱柔性机械臂凭借更为灵活、弯曲能力更强的特性，在诸如军事侦查、管道工作、辅助医疗检查、核电设备维护等等环境中有着广阔的发展前景，但是在一些特定工作场景下，当前常用的多维力传感器、光纤布拉格光栅传感器、电磁传感器等难以安装，或是易受干扰难以工作，这极大限制了绳驱柔性机械臂在复杂环境下的感知能力与力位混合控制方面的应用。

发明内容

本发明提供一种无外部传感器的绳驱柔性机械臂的末端力感知方法及装置，旨在至少解决现有技术中存在的技术问题之一，例如使绳驱柔性机械臂在末端不能安装力传感器且不借用其他外部传感器的情况下仍然能够实现对末端力信息的准确感知。

本发明的技术方案为一种无外部传感器的绳驱柔性机械臂的末端力感知方法。该方案所基于的硬件平台的机械臂包括驱动箱和串联的多段的臂杆组件，所述的驱动箱包括多个电机和传动机构，所述的机械臂的末端设置在末段臂杆组件的臂杆上，每个臂杆组件包括中心块和臂杆，每个臂杆组件中的臂杆与中心块通过轴承连接，并且每个臂杆组件中的臂杆通过各自的驱动绳索连接至所述的驱动箱的传动机构。所述方法包括以下步骤：

A、采集绳驱柔性机械臂的当前关节角度值，基于电机旋转位置和传动机构到驱动绳索运动长度的转换关系，实时采集每个电机的驱动反馈电流，建立与该电机关联的驱动绳索的拉力计算，并且建立机械臂的杆件坐标系；

B、基于末段的臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力 F_ex1；

C、基于倒数两段臂杆的动力学平衡方程与力矩平衡方程，计算倒数两段臂杆所关联的末端作用力F_ex2；

D、以迭代的方式，基于倒数n段臂杆的动力学平衡方程与力矩平衡方程，计算倒数n段臂杆所关联的末端作用力F_exn，其中n＞2；

E、如果前后两次计算的末端作用力的误差低于阈值或者迭代数等于臂杆组件的段数，则停止迭代并输出当前计算的末端作用力作为末端感知力，否则返回步骤C。

进一步，所述步骤A包括：根据滚丝杠机构输入力矩与输出拉力的关系，计算与滚丝杠机构连接的驱动绳索的输出拉力：

其中，L为丝杠导程，η为传动效率系数，i_m为电机工作电流，K_m为转换常数，与电机自身性能相关；

根据绳驱柔性机械臂的运动学模型，利用DH参数实时建立绳驱柔性机械臂的杆件坐标系，计算所述绳驱柔性机械臂在运动过程中各个臂杆组件中的运动臂杆与中心块相对于自身固连坐标系的角速度、角加速度以及惯性张量阵；

建立绳索通过臂杆组件中一个绳孔后的相邻两段的张力关系方程：

T₂＝μ(θ_c,sgn(v_c))T₁+F_v(θ_c)v_c

其中，T₁是过绳孔前的绳索张力，T₂是过绳孔后的绳索张力，v_c是过绳孔时绳索的速度，θ_c是绳索过绳孔时相邻两段的夹角，μ和F_v分别为库伦摩擦系数与粘滞摩擦系数；

根据驱动绳索通过臂杆组件中的绳孔的张力关系，采集驱动箱内每根驱动绳索的实际拉力值，以计算每个臂杆受到的绳索张力。

进一步，所述步骤B包括：通过末段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；构建末段臂杆组件的臂杆和中心块在中心块坐标系上的动力学平衡方程：

其中，I_L是臂杆相对于中心块坐标系的惯性张量阵，I_B是中心块相对于中心块坐标系的惯性张量阵，ω_L与ω_B分别是臂杆与中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M_c为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；

根据以上动力学平衡方程计算M_c，并计算绳索力力矩、重力力矩、轴承摩擦力矩；

给定中心块和臂杆的坐标系的x轴方向都为对应的中心块和臂杆的串联方向，而臂杆间相互作用力矩方向与中心块坐标系x轴方向平行，通过以下方程计算末端作用力在臂杆坐标系上的y轴分量和z轴分量：

其中，M_cable、M_G、M_bearing分别是绳索力矩、重力力矩与轴承摩擦力矩，F_y0、F_z0是初始末端接触力F_ex0在臂杆坐标系y轴和z轴的初始分量，求得初始末端作用力为： F_ex1＝[0,F_y0,F_z0] 。

进一步，所述步骤C包括：通过倒数两段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；将倒数两段臂杆组件作为整体，构建这两段臂杆组件的中心块坐标系的动力学平衡方程：

其中，I_L1、I_L2分别是末段和倒数第二段臂杆相对于关节中心块坐标系的惯性张量阵， I_B1、I_B2分别是末段和倒数第二段中心块相对于关节中心块坐标系的惯性张量阵，ω_L1、ω_L2与ω_B1、ω_B2分别是臂杆与中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M_c为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；

与对末端臂杆建立以下力矩平衡方程，计算末端作用力在末段臂杆坐标系上的x轴分量：

其中M_cable、M_G、M_bearing分别是绳索力矩、重力力矩与轴承摩擦力矩，F_x0是初始末端接触力F_ex1在臂杆坐标系x轴的初始分量，求得当前迭代的末端作用力为：F_ex2＝[F_x0,F_y0,F_z0]。

进一步，所述步骤D包括：通过倒数n段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；将倒数n个的臂杆视为一个整体，构建动力学平衡方程：

其中，I_Li是臂杆相对于关节中心块坐标系的惯性张量阵，I_Bi是中心块相对于关节中心块坐标系的惯性张量阵，ω_Li和ω_Bi分别是臂杆与中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M_c为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；

与对末端臂杆建立以下力矩平衡方程，以计算末端作用力F_exn：

其中，M_cable、M_G、M_bearing分别是绳索力矩、重力力矩与轴承摩擦力矩。

进一步，所述步骤D还包括：根据绳索张力对臂杆的力臂在基坐标系上的矢量

并且

根据基坐标系上的绳索方向向量与绳孔两端绳索张力关系方程，计算绳索张力对臂杆造成的在中心块固连坐标系上的力矩为：

其中，

是绳索张力在基坐标系的矢量表示；

计算当前臂杆重力相对于中心块固连坐标系的力矩：

计算当前关节轴承负载在中心块固连坐标系上的矢量为：

根据轴承摩擦力矩计算公式，计算轴承摩擦力矩为：

其中，μ为轴承摩擦系数，D为轴承内径大小，

分别是轴承负载

在中心块固连坐标系x、y、z轴方向分量。

进一步，所述步骤D还包括：分别计算末段臂杆在中心块坐标系上的惯性张量阵、角速度与角加速度：

根据牛顿欧拉动力学方程，构建末段臂杆在中心块固连坐标系上的动力学平衡方程：

其中，

是臂杆相对于中心块坐标系的惯性张量阵，

是中心块相对于中心块坐标系的惯性张量阵，

与

分别是臂杆与中心块在中心块坐标系的角速度矢量，

为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩。

进一步，所述步骤E包括：对当前计算的末端作用力F_exn与上一次计算的末端作用力F_ex(n-1)的差值计算范数值；如果所述范数值低于一阈值，则停止迭代运算并输出当前计算的末端作用力作为末端感知力。

本发明的技术方案还涉及一种计算机装置，包括存储器和处理器，所述处理器执行储存在所述存储器中的计算机程序时实施上述的方法。

本发明的技术方案还涉及一种计算机可读存储介质，其上储存有计算机程序指令，所述计算机程序指令被处理器执行时实施上述的方法。

本发明的有益效果为：

通过计算程序迭代算出机械臂受到的末端力，使绳驱柔性机械臂在末端不能安装力传感器且没有外部传感器的情况下仍然能够实现对末端力信息的准确感知，以实现对绳驱柔性机械臂的力控制等。

附图说明

图1所示为绳驱柔性机械臂的机构模型图。

图2所示为根据本发明的实施例的方法流程图。

图3所示为末段臂杆组件的受力分析图。

图4所示为倒数后两段的臂杆组件的受力分析图。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。

需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。本文所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本文所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。本文所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。

参照图1，根据本发明的方法和计算机装置用在一种无外部传感器绳驱柔性机械臂(也称绳驱柔性机器人)上，外部传感器不包括伺服电机内部的编码器。所述机械臂包括：驱动箱100、多个中心块200、多个臂杆300和多条驱动绳索400。所述的驱动箱包括多个电机和传动机构。每个中心块和臂杆组成一个臂杆组件，中心块和臂杆通过轴承枢转地连接。中心块200是十字形的，使得每两段臂杆组件之间连接形成一个二自由度的关节，每个关节一般由三根驱动绳索控制其运动。且所有的驱动绳索分别从臂杆底部穿过多个臂杆绳孔至所述绳索直接驱动的臂杆下端，使得可以通过驱动绳索拉伸或放松来驱动对应的臂关节运动。驱动绳索由传动机构带动进行线性移动。其中，传动机构可以是滚珠丝杆螺母机构。其余关于绳驱柔性机械臂的细节描述可以参照本申请人在先的一个名称为“一种带末端力反馈的柔性机械臂”的公开号为CN111993400A的发明公布文件。考虑到专利审查的节约原则，将该发明公布文件以全文引用的方式并入本文。根据本发明的方法和装置可以用于该公布文件中的柔性机械臂去除末端力反馈传感器以及关节编码器的情况。

此外，在绳驱柔性臂驱动箱内部对每根驱动绳都安装有拉力传感器实时采集绳索拉力值。

在一些实施例中，根据本发明的方法包括以下步骤：建立电机编码器反馈的旋转位置到绳索运动长度的转换关系；建立电机驱动反馈电流到绳索拉力的转换关系；建立机械臂的杆件坐标系；建立末段臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力 F_ex1；建立倒数两段臂杆的动力学平衡方程与力矩平衡方程，计算F_ex2；建立倒数n段臂杆的动力学平衡方程与力矩平衡方程，计算末端作用力F_exn；判断前后两个计算的末端作用力误差是否低于阈值，或者迭代次数是否等于臂杆数，如果是，则停止迭代。进一步如图2的流程图所示，在一个实施例中，根据本发明的方法包括如下程序步骤：

S100、设置段数变量n的初始值为1；

S110、初始化将末端作用力为F_ex0＝[0,0,0]；

S120、将倒数n个臂杆与中心块作为整体处理，建立计算模型子系统；

S130、采集中心块上的传感器数据，计算臂杆受到的绳索作用力；

S140、计算各臂杆相对于第n中心块坐标系的角速度、角加速度、惯性张力阵；

S150、基于当前子系统的牛顿-欧拉(Newton-Euler)动力学平衡方程，建立与末段臂杆受到的外力力矩M_c的关联计算；

S160、进行重力力矩、绳索作用力力矩和轴承摩擦力力矩的关联计算；

S170、基于力矩平衡方程，计算倒数n段的计算末端作用力F_exn；

S180、使段数变量n加1；

S190、对当前计算的末端作用力F_exn与上一次计算的末端作用力F_ex(n-1)的差值计算范数值，如果所述范数值低于一阈值(比如0.01)或者n达到臂杆段数，则停止迭代运算并输出当前计算的末端作用力作为末端感知力，否则返回步骤S120，进行下一次迭代运算。

下面参照图3和图4，通过详细的步骤来解释上述的方法流程细节。

通过电机驱动器反馈电流推算绳索拉力

根据驱动箱100中的丝杠传动公式：

其中，ΔL为绳长变化量，Δθ_m为电机旋转角度值，S为滚珠丝杠导程，η为电机减速比。

根据上述公式计算得到每条驱动绳索的绳长变化量，根据机械臂驱动空间到关节空间的转换关系计算求得臂当前关节角度值，进而可以求得各关节的角速度、角加速度值。

建立电机驱动器反馈电流到绳索拉力的转换关系。根据电机输出力矩与电机工作电流的关系：

T_m＝K_m·i_m

其中，T_m为电机的输出力矩值，i_m为电机工作电流，K_m为转换常数，与电机自身性能相关。

根据滚珠丝杠机构输入力矩与输出拉力的关系(滚珠丝杠，忽略摩擦力矩)：

其中，L为丝杠导程，η为传动效率系数。

可得电机驱动器反馈电流到绳索拉力的转换公式为：

根据绳驱柔性机械臂的运动学模型

利用DH参数实时建立绳驱柔性机械臂的杆件坐标系，计算得到所述绳驱柔性机械臂在运动过程中各个运动臂杆与中心块相对于自身固连坐标系的角速度、角加速度以及惯性张量。

上述柔性机械臂臂杆与中心块相对于自身固连坐标系的角速度、角加速度根据牛顿-欧拉迭代法计算：

臂杆与中心块相对于自身固连坐标系的角速度与角加速度的计算公式为：

其中，²ⁿω_Ln、²ⁿα_Ln表示臂杆n在自身固连坐标系{2n}上的角速度矢量与角加速度矢量，^2n-1ω_Bn、^2n-1α_Bn表示中心块n在自身固连坐标系{2n-1}上的角速度矢量与角加速度矢量。

计算初始末端外力F_ex1

由于末端实际外力对末段臂杆的力矩在末段臂杆固连坐标系的x轴方向的分量为0，因此忽略所计算的F_ex1在末段臂杆固连坐标系x轴方向分量。

假设绳驱柔性机械臂共有n段臂杆，如图3，将末段臂杆n(1)与中心块n(2)视为整体分析。末段臂杆固连坐标系为{O_2n}，末端臂杆关节处的中心块固连坐标系为{O_2n-1}，倒数第二段臂杆固连坐标系为{O_2n-2}。计算驱动末端臂杆的绳索在基坐标系上的方向向量：

其中，l指臂杆上下关节距离，d指关节处上下臂杆表面距离，r指绳孔分布圆半径，β_k表示绳孔在对应坐标系上的位置，

是臂杆固连坐标系与基坐标系的旋转矩阵，

是中心块固连坐标系与基坐标系的旋转矩阵。

建立绳索通过一个绳孔后的相邻两段的张力关系方程：

T₂＝μ(θ_c,sgn(v_c))T₁+F_v(θ_c)v_c

其中，T₁表示过绳孔前的绳索张力，T₂表示过绳孔后的绳索张力，v_c表示过绳孔时绳索的速度，θ_c表示绳索过绳孔时相邻两段的夹角，μ和F_v分别为库伦摩擦系数与粘滞摩擦系数。

计算绳索张力对臂杆的力臂在基坐标系上的矢量为：

根据以上计算得到基坐标系上的绳索方向向量与绳孔两端绳索张力关系方程，可计算绳索张力对臂杆造成的在中心块固连坐标系上的力矩为：

其中，

是绳索张力在基坐标系的矢量表示。

计算当前臂杆重力相对于中心块固连坐标系的力矩：

计算当前关节轴承负载在中心块固连坐标系上的矢量表示为：

根据轴承摩擦力矩计算公式，轴承摩擦力矩为：

其中，μ为轴承摩擦系数，D为轴承内径大小，

分别是轴承负载

在中心块固连坐标系x、y、z轴方向分量。

计算末段臂杆在中心块坐标系上的惯性张量阵、角速度与角加速度：

根据牛顿-欧拉动力学方程，构建末段臂杆在中心块固连坐标系上的动力学平衡方程：

其中，

是臂杆相对于中心块坐标系的惯性张量阵，

是中心块相对于中心块坐标系的惯性张量阵，

与

分别是臂杆与中心块在中心块坐标系的角速度矢量，

为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩。

由于臂杆间相互作用力矩在中心块固连坐标系x轴方向为0，可以列出以下力矩平衡方程计算末端作用力在臂杆坐标系上的y轴分量和z轴分量：

计算初始末端外力为：F_ex1＝[0,F_y0,F_z0]'。

计算倒数两段臂杆组件的末端外力F_ex2

将末端两个臂杆(臂杆n(1)、中心块n(2)、臂杆n-1(3)、中心块n-1(4))视为一个整体分析。假设倒数第二段臂杆固连坐标系为{O_2n-2}，倒数第二段臂杆关节处的中心块固连坐标系为{O_2n-3}。计算驱动末端臂杆的绳索在基坐标系上的方向向量：

由于经过倒数第二臂杆的还有3n-2、3n-1、3n三条绳，根据绳索过绳孔两端张力关系方程，三条绳经过所述臂杆上下两个面的绳孔时的张力矢量分别为：

分别是过渡绳索在关节n，臂杆n-1之间、关节n-1上的方向矢量。根据绳索过绳孔两端张力关系方程，计算驱动绳索张力与过渡绳索张力对臂杆的力矩在基坐标系上的矢量表示为：

其中

分别是过渡绳索在通过臂杆n-1的上下面绳孔绳索张力的合力。

计算当前臂杆n、臂杆n-1和中心块n重力相对于中心块n-1固连坐标系的力矩：

计算当前关节轴承负载在中心块固连坐标系上的矢量表示为：

根据轴承摩擦力矩计算公式，轴承摩擦力矩为：

其中，μ为轴承摩擦系数，D为轴承内径大小，

分别是轴承负载

在中心块n-1固连坐标系x、y、z轴方向分量。

与末段臂杆同理，计算末段臂杆在中心块坐标系上的惯性张量阵、角速度与角加速度后，根据牛顿-欧拉动力学方程，构建末段臂杆在中心块固连坐标系上的动力学平衡方程：

由于上述忽略了外力在坐标系{O_n}x轴的分量，列出以下力矩平衡方程：

其中F_x0是末端作用力在坐标系{O_2n}的x轴分量作用在坐标系{O_2n-2}上的y、z分量。

计算倒数两段的末端外力为：F_ex2＝[F_x0,F_y0,F_z0]'。

迭代计算倒数两段臂杆组件的末端外力F_exn

与上述步骤采用相同的方法进行倒数第n段臂杆组件的迭代运算，将

代入子系统 n+1的力矩平衡方程，补偿计算末端外力在末段臂杆坐标系上的x分量的误差。

即是，将倒数n个臂杆视为一个整体，同理构建其动力学平衡方程：

计算得M_c。同理，建立以下方程计算末端作用力：

重复以上步骤，当F_exn与F_ex(n-1)的差值得模小于阈值或者循环次数超过一定值时，结束迭代循环。

应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。

计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。

以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/ 或实施方式可以有各种不同的修改和变化。

Claims (10)

1.一种无外部传感器的绳驱柔性机械臂的末端力感知方法，所述的机械臂包括驱动箱和串联的多段的臂杆组件，所述的驱动箱包括多个电机和传动机构，所述的机械臂的末端设置在末段臂杆组件的臂杆上，每个臂杆组件包括中心块和臂杆，每个臂杆组件中的臂杆与中心块通过轴承连接，并且每个臂杆组件中的臂杆通过各自的驱动绳索连接至所述的驱动箱的传动机构，

其特征在于，所述方法包括以下步骤：

A、采集绳驱柔性机械臂的当前关节角度值，基于电机旋转位置和传动机构到驱动绳索运动长度的转换关系，实时采集每个电机的驱动反馈电流，建立与该电机关联的驱动绳索的拉力计算，并且建立机械臂的杆件坐标系；

B、基于末段的臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力F_ex1；

C、基于倒数两段臂杆的动力学平衡方程与力矩平衡方程，计算倒数两段臂杆所关联的末端作用力F_ex2；

D、以迭代的方式，基于倒数n段臂杆的动力学平衡方程与力矩平衡方程，计算倒数n段臂杆所关联的末端作用力F_exn，其中n>2；

E、如果前后两次计算的末端作用力的误差低于阈值或者迭代数等于臂杆组件的段数，则停止迭代并输出当前计算的末端作用力作为末端感知力，否则返回步骤C。

2.根据权利要求1所述的方法，其中所述的传动机构为滚丝杠机构，所述步骤A包括：

根据滚丝杠机构输入力矩与输出拉力的关系，计算与滚丝杠机构连接的驱动绳索的输出拉力：

其中，L为丝杠导程，η为传动效率系数，i_m为电机工作电流，K_m为转换常数，与电机自身性能相关；

根据绳驱柔性机械臂的运动学模型，利用DH参数实时建立绳驱柔性机械臂的杆件坐标系，计算所述绳驱柔性机械臂在运动过程中各个臂杆组件中的运动臂杆与中心块相对于中心块坐标系的角速度、角加速度以及惯性张量阵；

建立绳索通过臂杆组件中一个绳孔后的相邻两段的张力关系方程：

T₂＝μ(θ_c，sgn(v_c))T₁+F_v(θ_c)v_c

其中，T₁是过绳孔前的绳索张力，T₂是过绳孔后的绳索张力，v_c是过绳孔时绳索的速度，θ_c是绳索过绳孔时相邻两段的夹角，μ和F_v分别为库伦摩擦系数与粘滞摩擦系数；

根据驱动绳索通过臂杆组件中的绳孔的张力关系，采集驱动箱内每根驱动绳索的实际拉力值，以计算每个臂杆受到的绳索张力。

3.根据权利要求1所述的方法，其中，所述步骤B包括：

通过末段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；

构建末段臂杆组件的臂杆和中心块在中心块坐标系上的动力学平衡方程：

其中，I_L是末段臂杆相对于中心块坐标系的惯性张量阵，I_B是末段中心块相对于中心块坐标系的惯性张量阵，ω_L与ω_B分别是末段臂杆与末段中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M_c为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；

根据以上动力学平衡方程计算M_c，并计算绳索力力矩、重力力矩、轴承摩擦力矩；

给定中心块和臂杆的坐标系的x轴方向都为对应的中心块和臂杆的串联方向，而臂杆间相互作用力矩方向与中心块坐标系x轴方向平行，通过以下方程计算末端作用力在臂杆坐标系上的y轴分量和z轴分量：

其中，M_cable、M_G、M_br分别是绳索力矩、重力力矩与轴承摩擦力矩，F_y0、F_z0是初始末端接触力F_ex0在臂杆坐标系y轴和z轴的初始分量，

是单位矢量，求得初始末端作用力为：

F_ex1＝[0，F_y0，F_z0]。

4.根据权利要求2或3所述的方法，其中，所述步骤C包括：

通过倒数两段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；

将倒数两段臂杆组件作为整体，构建这两段臂杆组件的中心块坐标系的动力学平衡方程：

其中，I_L1、I_L2分别是末段和倒数第二段臂杆相对于中心块坐标系的惯性张量阵，I_B1、I_B2分别是末段和倒数第二段中心块相对于中心块坐标系的惯性张量阵，ω_L1、ω_L2与ω_B1、ω_B2分别是末段和倒数第二段臂杆与末段和倒数第二段中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M_c为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；

对末端臂杆建立以下力矩平衡方程，计算末端作用力在末段臂杆坐标系上的x轴分量：

其中，M_cable、M_G、M_br分别是绳索力矩、重力力矩与轴承摩擦力矩，F_x0是初始末端接触力F_ex1在臂杆坐标系x轴的初始分量，F_y0、F_z0是初始末端接触力F_ex0在臂杆坐标系y轴和z轴的初始分量，求得当前迭代的末端作用力为：

F_ex2＝[F_x0，F_y0，F_z0]。

5.根据权利要求4所述的方法，其中，所述步骤D包括：

通过倒数n个臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；

将倒数n个的臂杆组件视为一个整体，构建动力学平衡方程：

其中，I_Li是倒数i个臂杆相对于中心块坐标系的惯性张量阵，I_Bi是倒数i个中心块相对于中心块坐标系的惯性张量阵，ω_Li和ω_Bi分别是倒数i个臂杆与倒数i个中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M_c为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；

对末端臂杆建立以下力矩平衡方程，以计算末端作用力F_exn：

其中，M_cable、M_G、M_bearing分别是绳索力矩、重力力矩与轴承摩擦力矩，

是单位矢量。

6.根据权利要求2所述的方法，其中，所述步骤D还包括：

计算驱动末端臂杆的绳索在基坐标系上的方向向量：

其中，l指臂杆上下关节距离，d指关节处上下臂杆表面距离，r指绳孔分布圆半径，β_k表示绳孔在对应坐标系上的位置，

是臂杆坐标系与基坐标系的旋转矩阵，

是中心块坐标系与基坐标系的旋转矩阵；

根据绳索张力对臂杆的力臂在基坐标系上的矢量

并且

根据基坐标系上的绳索方向向量与绳孔两端绳索张力关系方程，计算绳索张力对臂杆造成的在中心块坐标系上的力矩为：

其中，

是绳索张力在基坐标系的矢量表示；

计算当前臂杆重力相对于中心块坐标系的力矩：

计算当前关节轴承负载在中心块坐标系上的矢量为：

根据轴承摩擦力矩计算公式，计算轴承摩擦力矩为：

其中，μ为轴承摩擦系数，D为轴承内径大小，

分别是当前关节轴承负载

在中心块坐标系x、y、z轴方向分量。

7.根据权利要求6所述的方法，其中，所述步骤D还包括：

分别计算末段臂杆在中心块坐标系上的惯性张量阵、角速度与角加速度：

根据牛顿欧拉动力学方程，构建末段臂杆在中心块坐标系上的动力学平衡方程：

其中，

是末段臂杆相对于中心块坐标系的惯性张量阵，

是末段中心块相对于中心块坐标系的惯性张量阵，

与

分别是末段臂杆与末段中心块在中心块坐标系的角速度矢量，

为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩。

8.根据权利要求1所述的方法，其中，所述步骤E包括：

对当前计算的末端作用力F_exn与上一次计算的末端作用力F_ex(n-1)的差值计算范数值；

如果所述范数值小于一阈值，则停止迭代运算并输出当前计算的末端作用力作为末端感知力。

9.一种计算机装置，包括存储器和处理器，其特征在于，所述处理器执行储存在所述存储器中的计算机程序时实施如权利要求1至8中任一项所述的方法。

10.一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如权利要求1至8中任一项所述的方法。

Images