腱驱动机械手末端接触力的估计方法技术

本发明专利技术公开了一种腱驱动机械手末端接触力的估计方法，本发明专利技术基于灵巧手的动力学与运动学模型，首先将灵巧手的动力学模型转化为状态方程，然后将状态方程离散化，最后通过粒子滤波的方法，估计灵巧手末端的接触力，从而获得对应的末端力矩以完成对于目标物体的抓取工作；本发明专利技术为机械臂、灵巧手提供了一种不需要基于力传感器的末端接触力估计的新思路，而且算法的实现易于理解，抗干扰能力强、精度高、适应性强，可以实现高精度腱驱动机械手末端接触力估计与控制；同时可以节省在灵巧手末端安装六维力传感器的空间，降低灵巧手的成本。降低灵巧手的成本。降低灵巧手的成本。

【技术实现步骤摘要】
腱驱动机械手末端接触力的估计方法


[0001]本专利技术涉及机械手末端接触力的估计及控制，属于机械手控制，接触力估计及控制等


技术介绍

[0002]为了避免灵巧手末端发生碰撞或者降低碰撞所带来的不良后果，就需要对灵巧手进行力控制，力控制首先需要获取灵巧手的受力信息，一般在灵巧手末端或者关节上安装六维力矩传感器来获取受力信息，但是这种方法有一些缺点，一是传感器价格偏高，影响整体成本；二是传感器会降低整体灵巧手系统的刚度，影响灵巧手系统的动力学建模；三是由于传感器会带来电噪声，因此就需要加入观测器来降低噪声带来的影响，这样也会增加系统的复杂性和响应速度。因此，无传感器的末端外力估计逐渐成为了科研工作者的研究重点。
[0003]灵巧手末端外力估计与关节外力矩估计是等价的，科研工作者提出了很多观测方法来估计多关节刚体外力。Hacksel等人首先使用观测器对外力进行估计，其基本思想是将观测器误差动力学视为一个由环境外力驱动的质量阻尼弹簧系统，通过刚体的实际位置误差来估计受到的外力情况。Luca等人提出了一种使用广义动量来进行外力估计的方法，并将其应用在DLR柔性多关节刚体的自动碰撞检测技术中。Chen等人提出一种非线性扰动观测器，通过选择设计参数来保证扰动观测器的性能。Jung等人针对多自由度灵巧手提出一种基于卡尔曼滤波器的外力估计方法，该方法不需要计算惯性矩阵的逆矩阵，基于广义动量构建表达式并离散化成适用于标准卡尔曼滤波的系统方程。之后，Capurso，Wahrburg基于Jung的工作进行了改进，提出了基于扩展卡尔曼滤波的外力估计方法。但是上述方法对模型的不确定性以及观测噪声的抗干扰能力较差，导致其外力估计效果较差。

技术实现思路

[0004]本专利技术的目的：针对腱驱动灵巧手设计一种末端与环境的接触力的估计算法。该方法基于模型估计外部接触力，能够降低灵巧手的成本；接触力估计抗干扰能力强，提高接触力估计精度，进而提高系统末端操作的力控制精度，减小从自由空间到约束空间的冲击，通过约束腱张力，降低腱的磨损，提高其寿命，最终能够有效的实现腱驱动机械手的操作控制，提高机械手的灵巧性和寿命。
[0005]本专利技术采用的方案：
[0006]一种腱驱动机械手末端接触力的估计方法，包括如下步骤：
[0007]步骤1：根据系统的动力学方程得到系统的状态方程；
[0008]步骤2：将步骤1得到的系统的状态方程离散化；
[0009]步骤3：根据步骤2的离散化后得到的状态方程与量测方程，基于系统的状态空间模型，根据蒙特卡洛抽样方法可以得到状态变量的初始参数样本，即可以得到初始粒子集；
[0010]步骤4：设定粒子滤波的结束条件，进行粒子滤波，得到系统的状态变量的最优估
计值；
[0011]步骤5：根据步骤4得到的最优估计值解算出灵巧手末端接触力的最优估计值。
[0012]进一步的，步骤1根据系统的动力学方程得到系统的状态方程具体包括如下步骤：
[0013]步骤1.1：在关节受到外力矩时，灵巧手指的动力学方程为：
[0014][0015]其中M(q)表示惯性矩阵，表示科氏力矩阵，G(q)表示重力项，q分别表示关节的加速度，速度，位移项，τ
e
表示关节受到的外力矩，u表示驱动力矩。
[0016]步骤1.2：确定系统的状态变量，分别为系统广义动量p以及τ
e
，p具体表示为如下公式：
[0017]p＝M(q)q
[0018]步骤1.3：对系统的广义动量进行求导可以得到：
[0019][0020]步骤1.4：记力矩的观测值为具体表示为：
[0021][0022]步骤1.5：将步骤1.4得到的结果代入到步骤1.3中有：
[0023][0024]步骤1.6：因为每个模型都有自己的不确定性，这里记模型的不正确定性为w
p
，实际上就是高斯分布的白色噪声；
[0025]步骤1.7：根据经典观测器思想。关节所受外力矩τ
e
可以表示为如下模型：
[0026][0027]一般的A
τ
为0，而w
τ
为高斯白色噪声；
[0028]步骤1.8：由此，就可以得到系统的状态方程：
[0029][0030]其中N是灵巧手指的自由度的个数，N
e
是外力矩的维数，一般为6；
[0031]步骤1.9：记系统的状态分别为p,τ
e
，记为x＝[pτ
e
]T
那么，系统的状态方程可以表示为：
[0032][0033]式中，步骤1.10：记A
τ
为0，那么记观测量为y观测方程可以表示为：
[0034][0035]式中v＝(0,R
c
)表示测量噪声项；
[0036]步骤1.11：由此就可以得到系统的测量方程为：
[0037]y＝C
c
x+v
[0038]其中，
[0039]进一步的步骤2中的系统状态方程离散化的具体步骤为：
[0040]步骤2.1：离散化后的状态方程与量测方程可以表示为：
[0041]与y
k+1
＝C
k
x
k
+v
k
[0042]其中，下标k，k+1分别表示当前时刻与下一时刻，A
k
，B
k
，C
k
表离散化系统的系数矩阵；
[0043]步骤2.2：离散化后的状态方程与量测方程的参数具体的求解方法如下：
[0044]步骤2.2.1：系统的状态方程的参数具体为：
[0045][0046]exp表示e的幂级数，Δt表示采样间隔；
[0047]步骤2.2.2：系统的量测方程的参数具体为：
[0048][0049]其中R
k
表示测量噪声的协方差矩阵；
[0050]步骤2.3：定义粒子滤波的参数，具体为：
[0051]Q
c
＝diag{0.0001,0.0001,0.0001,10,10,10}，R
c
＝diag{0.01,0.01,0.01}；
[0052]进一步的，步骤3的粒子初始化具体步骤为：
[0053]步骤3.1：贝叶斯预测与更新方程：
[0054]步骤3.1.1：贝叶斯预测方程为：
[0055]P(X
k
|Y
1:k
‑1)＝∫P(X
k
|X
k
‑1)P(X
k
‑1|Y
1:k
‑1)dX
k
‑1[0056]步骤3.1.2：贝叶斯更新方程为：
[0057][0058]式中，前一时刻的后验概率P(X
k
‑1|Y
k
...

【技术保护点】

【技术特征摘要】
1.一种腱驱动机械手末端接触力的估计方法，其特征在于，包括以下步骤：步骤1：根据系统的动力学方程以及待估计的末端接触力的值，确定系统的状态值，并且推导系统的状态方程与量测方程；步骤2：将所述状态方程与量测方程离散化；步骤3：根据所述离散化后的状态方程与量测方程，基于系统的状态空间模型，根据蒙特卡洛抽样方法可以得到状态变量的初始参数样本，即得到系统的初始粒子集；步骤4：设定粒子滤波的结束条件，进行粒子滤波，得到系统的状态变量的最优估计值；步骤5：根据所述最优估计值解算出灵巧手末端接触力的最优估计值。2.根据权利要求1中所述的一种腱驱动机械手末端接触力估计算法，其特征在于，步骤1确定系统的状态值分别为：p与τ
e
，然后根据系统的动力学方程计算系统的状态方程。3.根据权利要求1中所述的一种腱驱动机械手末端接触力估计算法，其特征在于，所述步骤2的具体步骤为：步骤2.1：根据系统的状态方程，计算离散化后的状态方程的参数值，得到离散化的状态方程；步骤2.2：根据系统的量测方程，计算离散化后的量测方程的参数值，得到离散化的量测方程；步骤2.3：根据系统离散化后的状态方程和量测方程，定义系统中的参数值。4.根据权利要求1所述的估计方法，其特征在于，所述步骤3中根据蒙特卡洛抽样的方法，得到状态变量的初始参数样本的具体步骤为：步骤3.1：基于贝叶斯理论，得到贝叶斯预测方程与贝叶斯更新方程；步骤3.2：基于蒙特卡洛方法，系统的初始粒子。5.根据权利要求1所述的估计方法，其特征在于，所述步骤4中粒子滤波的具体步骤为：步骤4.1：重要性采样，具体步骤为：步骤4.1.1：在一个已知的可以采样的分布中去采样，得到待估计状态量的期望值；步骤4.1.2：解算每个粒子的权重值w(X
k
)；步骤4.1.3：将粒子的权重归一化；步骤4.2：递推式计算粒子权重，具体步骤为：步骤4.2.1：根据递推式方法求解，粒子权重具体表示为：式中P(X

【专利技术属性】
技术研发人员：王小涛，范彪，汪海洋，王邢波，崔宇新，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：