绳驱柔性机械臂的末端力感知方法及装置制造方法及图纸

本发明专利技术涉及一种绳驱柔性机械臂的末端力感知方法及装置。其中的方法包括建立机械臂的杆件坐标系；建立末段臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力；建立倒数两段臂杆的动力学平衡方程与力矩平衡方程，计算此时的末端作用力；建立倒数多段臂杆的动力学平衡方程与力矩平衡方程，迭代计算末端作用力；判断前后两个计算的末端作用力误差是否低于阈值，或者下一迭代次数是否大于臂杆数，如果是，则停止迭代。其中的装置包括存储器和处理器，该处理器在执行储存在存储器的指令时实施所述方法。通过程序迭代算出机械臂受到的末端力，使绳驱柔性机械臂在末端不能安装力传感器的情况下仍然能够实现对末端力信息的准确感知。息的准确感知。息的准确感知。

【技术实现步骤摘要】
绳驱柔性机械臂的末端力感知方法及装置


[0001]本专利技术属于机械臂
，涉及一种绳驱柔性机械臂的末端力感知方法及装置。

技术介绍

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

技术实现思路

[0003]本专利技术提供一种绳驱柔性机械臂的末端力感知方法及装置，旨在至少解决现有技术中存在的技术问题之一，例如使绳驱柔性机械臂在末端不能安装力传感器的情况下仍然能够实现对末端力信息的准确感知。
[0004]本专利技术的技术方案为一种绳驱柔性机械臂的末端力感知方法。该方案所基于的硬件平台的机械臂包括驱动箱和串联的多段的臂杆组件，所述的机械臂的末端设置在末段臂杆组件的臂杆上，每个臂杆组件包括中心块和臂杆，每个臂杆组件中的臂杆与中心块通过轴承连接，并且每个臂杆组件中的臂杆通过各自的驱动绳索连接至所述的驱动箱，在所述的驱动箱内对驱动绳索设有拉力传感器以实时采集每根驱动绳索的拉力值。方法包括以下步骤：
[0005]A、建立机械臂的杆件坐标系，通过拉力传感器采集每根驱动绳索的拉力值以计算绳索张力施加给臂杆的相对于其连接的中心块的坐标系的绳索力的力矩，
[0006]B、基于末段的臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力F
ex1
；
[0007]C、基于倒数两段臂杆的动力学平衡方程与力矩平衡方程，计算倒数两段臂杆所关联的末端作用力F
ex2
；
[0008]D、以迭代的方式，基于倒数n段臂杆的动力学平衡方程与力矩平衡方程，计算倒数n段臂杆所关联的末端作用力F
exn
，其中n＞2；
[0009]E、如果前后两次计算的末端作用力的误差低于阈值或者下一个迭代数大于臂杆组件的段数，则停止迭代并输出当前计算的末端作用力作为末端感知力，否则返回步骤C。
[0010]进一步，所述步骤A包括：根据绳驱柔性机械臂的运动学模型，利用DH参数实时建立绳驱柔性机械臂的杆件坐标系，计算所述绳驱柔性机械臂在运动过程中各个臂杆组件中的运动臂杆与中心块相对于自身固连坐标系的角速度、角加速度以及惯性张量阵；
[0011]建立绳索通过臂杆组件中一个绳孔后的相邻两段的张力关系方程：
[0012]T2＝μ(θ
c
,sgn(v
c
))T1+F
v
(θ
c
)v
c
[0013]其中，T1是过绳孔前的绳索张力，T2是过绳孔后的绳索张力，v
c
是过绳孔时绳索的速度，θ
c
是绳索过绳孔时相邻两段的夹角，μ和F
v
分别为库伦摩擦系数与粘滞摩擦系数；
[0014]根据驱动绳索通过臂杆组件中的绳孔的张力关系，采集驱动箱内每根驱动绳索的实际拉力值，以计算每个臂杆受到的绳索张力。
[0015]进一步，所述步骤B包括：通过末段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；构建末段臂杆组件的臂杆和中心块在中心块坐标系上的动力学平衡方程：
[0016][0017]其中，I
L
是臂杆相对于中心块坐标系的惯性张量阵，I
B
是中心块相对于中心块坐标系的惯性张量阵，ω
L
与ω
B
分别是臂杆与中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M
c
为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；
[0018]根据以上动力学平衡方程计算M
c
，并计算绳索力力矩、重力力矩、轴承摩擦力矩；
[0019]给定中心块和臂杆的坐标系的x轴方向都为对应的中心块和臂杆的串联方向，而臂杆间相互作用力矩方向与中心块坐标系x轴方向平行，通过以下方程计算末端作用力在臂杆坐标系上的y轴分量和z轴分量：
[0020][0021]其中，M
cable
、M
G
、M
bearing
分别是绳索力矩、重力力矩与轴承摩擦力矩，F
y0
、F
z0
是初始末端接触力F
ex0
在臂杆坐标系y轴和z轴的初始分量，求得初始末端作用力为：
[0022]进一步，所述步骤C包括：通过倒数两段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；将倒数两段臂杆组件作为整体，构建这两段臂杆组件的中心块坐标系的动力学平衡方程：
[0023][0024]其中，I
L1
、I
L2
分别是末段和倒数第二段臂杆相对于关节中心块坐标系的惯性张量阵，I
B1
、I
B2
分别是末段和倒数第二段中心块相对于关节中心块坐标系的惯性张量阵，ω
L1
、ω
L2
与ω
B1
、ω
B2
分别是臂杆与中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M
c
为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；
[0025]与对末端臂杆建立以下力矩平衡方程，计算末端作用力在末段臂杆坐标系上的x轴分量：
[0026][0027]其中M
cable
、M
G
、M
bearing
分别是绳索力矩、重力力矩与轴承摩擦力矩，F
x0
是初始末端接触力F
ex1
在臂杆坐标系x轴的初始分量，求得当前迭代的末端作用力为：F
ex2
＝[F
x0
,F
y0
,F
z0
]。
[0028]进一步，所述步骤D包括：通过倒数n段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；将倒数n个的臂杆视为一个整体，构建动力学平衡方程：
[0029][0030]其中，I
Li
是臂杆相对于关节中心块坐标系的惯性张量阵，I
Bi
是中心块相对于关节中心块坐标系的惯性张量阵，ω
Li
和ω
Bi
分别是臂杆与中心块在中心块坐标系的角速度矢量，通过旋转编码器获取的旋转角速度计算获得，M
c
为末段臂杆受到的外力力矩，包括绳索力力矩、重力力矩、臂杆间相互作用力矩、轴承摩擦力矩和末端作用力力矩；
[0031]与本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种绳驱柔性机械臂的末端力感知方法，所述的机械臂包括驱动箱和串联的多段的臂杆组件，所述的机械臂的末端设置在末段臂杆组件的臂杆上，每个臂杆组件包括中心块和臂杆，每个臂杆组件中的臂杆与中心块通过轴承连接，并且每个臂杆组件中的臂杆通过各自的驱动绳索连接至所述的驱动箱，在所述的驱动箱内对驱动绳索设有拉力传感器以实时采集每根驱动绳索的拉力值，其特征在于，所述方法包括以下步骤：A、建立机械臂的杆件坐标系，通过拉力传感器采集每根驱动绳索的拉力值以计算绳索张力施加给臂杆的相对于其连接的中心块的坐标系的绳索力的力矩，B、基于末段的臂杆与中心块的动力学平衡方程与力矩平衡方程，计算初始末端作用力F
ex1
；C、基于倒数两段臂杆的动力学平衡方程与力矩平衡方程，计算倒数两段臂杆所关联的末端作用力F
ex2
；D、以迭代的方式，基于倒数n段臂杆的动力学平衡方程与力矩平衡方程，计算倒数n段臂杆所关联的末端作用力F
exn
，其中n＞2；E、如果前后两次计算的末端作用力的误差低于阈值或者下一个迭代数大于臂杆组件的段数，则停止迭代并输出当前计算的末端作用力作为末端感知力，否则返回步骤C。2.根据权利要求1所述的方法，其中，所述步骤A包括：根据绳驱柔性机械臂的运动学模型，利用DH参数实时建立绳驱柔性机械臂的杆件坐标系，计算所述绳驱柔性机械臂在运动过程中各个臂杆组件中的运动臂杆与中心块相对于自身固连坐标系的角速度、角加速度以及惯性张量阵；建立绳索通过臂杆组件中一个绳孔后的相邻两段的张力关系方程：T2＝μ(θ
c
,sgn(v
c
))T1+F
v
(θ
c
)v
c
其中，T1是过绳孔前的绳索张力，T2是过绳孔后的绳索张力，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
bearing
分别是绳索力矩、重力力矩与轴承摩擦力矩，F
y0
、F
z0
是初始末端接触力F
ex0
在臂杆坐标系y轴和z轴的初始分量，求得初始末端作用力为：F
ex1
＝[0,F
y0
,F
z0
]。4.根据权利要求2或3所述的方法，其中，所述步骤C包括：通过倒数两段臂杆组件的中心块上设置的旋转编码器获取中心块相对于臂杆的旋转角速度；将倒数两段臂杆组件作为整体，构建这两段臂杆组件的中心块坐标系的动力学平衡方程：其中，I
L1
、I
L2

【专利技术属性】
技术研发人员：徐文福，王封旭，杨太玮，袁晗，梁斌，
申请(专利权)人：哈尔滨工业大学深圳，
类型：发明
国别省市：