一种基于迭代优化的机械臂接触力与环境刚度联合估计方法

本发明涉及一种基于迭代优化的机械臂接触力与环境刚度联合估计方法，适用于无传感器情形下的机械臂控制系统，属于机械臂力触觉反馈与估计领域。

背景技术：

1、机械臂触觉反馈是指机械臂通过感知与处理外部环境中的力与触觉信息，实现与环境的互动与交互。机械臂触觉反馈的传统实现方法主要为依靠触觉传感器等设备获取外界力的信息，其中触觉传感器多安装于机械臂的末端执行器或关节处，用于获取机械臂与物体接触时产生的力、接触区域分布、物体形状等信息。力触觉传感器通常能够提供较为准确的触觉力数据，但通常存在成本高、尺寸大、延迟较高等问题。因此，在无传感器情形下进行力觉和触觉的精确估计与反馈至关重要。

2、中国发明专利cn202011528330.x中设计了一种基于振动阵列的触觉刺激系统及方法，根据原始传达数据和初始化参数生成显示原始传达数据轮廓的原始数据点阵，同时利用振动阵列指令生成装置将原始数据点阵中的信息数据转换成控制指令发送给触觉刺激装置，从而使得振动阵列相应的刺激阵子起振，并对原始传达数据轮廓进行展示，以实现信息传递。但是该方法利用的振动机制有可能引入额外干扰，在没有滤除系统本身外干扰的情形下，触觉信息测量的精确性有可能受到影响。

3、中国发明专利cn201710834869.x中介绍了一种机械臂外力估计方法及装置，通过对机械臂动力学建模以及使用卡尔曼滤波器进行外力估计，避免了额外力传感器的使用；通过监督学习模型进行补偿，显著减小了机械臂动力学模型误差；通过引入卡尔曼滤波器，显著提升了外力估计在观测噪声和模型误差存在情况下的鲁棒性。但是该方法利用的卡尔曼滤波器依赖于单一量测值，有可能在较大程度上受到量测噪声的影响，机械臂外力估计的准确性有可能无法得到保证。

4、上述无传感器的力估计方法多是基于模型、算法或规则的推断，在数据中存在噪声、环境刚度未知等干扰的情形下，精确性和准确性方面通常有所欠缺，算法稳定性也有待商榷。

技术实现思路

1、因此，针对这一问题，本发明提供了一种可行的解决方法。

2、本发明的技术解决问题是：克服现有技术的不足，提供一种基于迭代优化的机械臂接触力与环境刚度联合估计方法，建立以环境刚度为参数的优化函数，通过rts平滑求解状态后验，根据优化函数批处理更新环境刚度估计值，从而实现无传感器力估计，提高机械臂与未知环境之间的交互性。

3、本发明的技术解决方案为：一种基于迭代优化的机械臂接触力与环境刚度联合估计方法，其实现步骤如下：

4、第一步，基于无力传感器机械臂系统与环境耦合动力学模型，结合系统状态当前量测值和环境刚度估计值，利用rts平滑求解状态变量及其协方差矩阵的后验估计值；

5、以为扩张状态，其中，pk为机械臂末端第k时刻的广义动量，ωk为第k时刻的扰动状态，建立如下无力传感器机械臂的环境耦合动力学模型：

6、

7、有如下参数矩阵：

8、

9、式中，分别表示第k时刻的增广系统状态矩阵、输入矩阵、输出矩阵，与环境刚度参数θ有关；yk表示第k时刻的测量信号；uk表示第k时刻的系统输入向量；ak,wk,ck分别表示广义动量与扰动状态的状态转移函数和测量函数；ek表示广义动量pk与环境刚度矩阵kθ间的系数矩阵；gk表示扰动状态ωk与广义动量pk间的系数矩阵；bk表示广义动量与输入向量间的输入矩阵，有：

10、ak＝in，wk＝ind，bk＝δt·in，ck＝in.

11、其中，δt、jk和分别为采样时间间隔、第k时刻机械臂雅可比矩阵和第k时刻系统惯性矩阵的逆，表示第k时刻雅各比矩阵的伪逆，表示n维单位阵，表示nd维单位阵；为环境刚度矩阵，θn.表示待估计的刚度阵中各维刚度参数；其中diag{·}表示由向量组成的对角阵；式中各矩阵维数匹配；

12、耦合动力学模型中的分别表示第k时刻的过程噪声和量测噪声，其分布为其中分别为过程噪声与量测噪声的协方差矩阵，表示随机变量服从高斯分布，具体结构有：

13、

14、其中，wk，νk，ξk分别表示第k时刻机械臂系统的过程噪声、干扰子系统输出噪声、干扰子系统过程噪声，vk表示第k时刻机械臂系统额量测噪声，有：

15、

16、其中，分别表示第k时刻机械臂系统的过程噪声协方差、干扰子系统输出噪声协方差、干扰子系统过程噪声协方差，表示第k时刻机械臂系统的量测噪声协方差。

17、在此基础上，结合当前量测值，利用rts平滑求解从时刻k-n到时刻k的时段内状态变量及其协方差矩阵的后验估计值，步骤如下：

18、(1)初始化前向卡尔曼滤波器：

19、

20、

21、式中，表示状态估计的均值，表示状态估计的协方差，表示第k-n步的前向滤波后验估计，m表示均值符号，p表示误差协方差阵符号，表示对随机变量求期望符号，(·)t表示向量的转置。

22、(2)对于第i步，i＝k-n+1,...,k(其中n是最后时刻)，执行前向卡尔曼滤波器：

23、

24、

25、

26、

27、

28、其中，为扩张状态系统的量测噪声协方差矩阵，kf,i为第i时刻的卡尔曼增益，为由前向卡尔曼滤波器求得的第i时刻状态估计先验值，为由前向卡尔曼滤波器求得的第i时刻状态估计后验值，θ*为利用第k-n-1时刻到第k-1时刻的量测求得的参数估计值，为用θ*求得的系统矩阵。

29、(3)初始化rts平滑器：

30、

31、

32、其中，(·)k|k表示利用第k-n时刻到第k时刻的量测进行rts平滑后得到的第k时刻状态估计值，mk|k为平滑后的状态估计均值，pk|k为平滑后的状态估计协方差；

33、(4)对于第i步，i＝k-1,...,k-n+1，执行rts平滑器：

34、

35、

36、

37、

38、

39、通过以上步骤，能够得出第i时刻的状态变量后验均值估计值第i时刻的状态变量协方差后验估计值pi|k、第i时刻与第i-1时刻的状态变量互协方差后验估计值pi,i-1|k，据此可推导关于待估计参数θ的优化函数。

40、第二步，基于无传感器机械臂的环境耦合动力学模型，建立第k时刻以待估计的环境刚度为参数的优化函数。首先计算对数似然函数，也即联合概率对数的条件期望：

41、

42、其中，xk-n:k是第k-n时刻到第k时刻的状态变量，y1:k是初始时刻到第k时刻的量测值；p(a|b；c)是参数取值为c时，在给定事件b发生的条件下，事件a发生的概率；p(a,b；c)是以c参数时，事件a,b同时发生的概率；∫·为积分符号，d·为全微分符号；表示似然函数，用于描述参数对观测数据的拟合程度；log(·)为对数符号；p(·)表示概率。

43、由贝叶斯公式与马尔可夫链的无后效性，可进一步计算联合概率的辅助递推形式表达式：

44、

45、其中，p(a|b)是在给定事件b发生的条件下，事件a发生的概率；σ(·)是求和符号；mk表示以θ为未知参数、状态变量与量测间截止到第k时刻的联合概率p(xk-n:k,y1:k；θ)的对数值，mk-n表示以θ为未知参数、状态变量与量测间截止到第k-n时刻的联合概率p(xk-n,y1:k-n；θ)的对数值，有：

46、mk-n＝log p(yk-n|xk-n)+log p(xk-n|y1:k-n-1；θ)+log p(y1:k-n-1)

47、仅保留与参数θ有关的项，可将用于优化求解的目标函数改写为：

48、

49、由状态变量的时间更新公式、以及过程噪声服从正态分布可知，条件概率满足：

50、

51、

52、其中，mi|i-1表示以θ为参数、xi-1为前提时，xi的均值，mk-n|k-n-1表示以θ为参数、y1:k-n-1为前提时，xk-n的均值，pk-n|k-n-1表示以θ为参数、y1:k-n-1为前提时，xk-n的协方差矩阵，有：

53、

54、其中，是未知参数θ对应的系统矩阵。结合正态分布的概率密度函数，优化函数表示为：

55、

56、其中，|·|表示矩阵的行列式。将rts平滑求解结果代入优化函数，可使环境刚度为优化函数中的唯一变量。

57、第三步，求解第二步中得到的优化函数的极大值点，作为当前的环境刚度估计值，并返回第一步直至迭代收敛；输出当前时刻的环境刚度估计值以及所对应的接触力估计值。其具体实现过程如下：

58、(1)求解第k时刻、第m步的极大似然估计值：

59、记当前步为第m步，通过梯度下降法求解第k时刻、第m步刚度参数的极大似然估计值由于用于优化求解的目标函数为凹函数，其相反数为凸函数，求解下式可得闭式解：

60、

61、其中，表示函数相对θ的偏导数，tr(·)是矩阵求迹符号，mθ,i|k表示以θ为参数、以y1:k为前提时与等均值相关的项，mθ,k-n表示以θ为参数、以y1:k-n-1为前提时与相关的项，有：

62、

63、

64、(2)迭代求解第k时刻的参数估计值：

65、基于第k时刻、第m步的刚度参数估计值将系统矩阵更新为并返回第一步迭代循环计算，直至达到预先设置的最大迭代次数或相邻两次迭代结果的误差小于给定阈值ε，即有终止条件并将当前迭代结果作为第k时刻的环境刚度估计值

66、(3)求解第k时刻的外力估计值：

67、基于上述过程，可输出第k时刻的环境刚度估计值进而求解并输出第k时刻机械臂末端所受触觉力的估计值

68、

69、其中，表示的第n1个分量。进而，基于第k时刻的刚度参数的估计值将系统矩阵更新为并将其作为返回第一步参与第k+1时刻的迭代与求解。

70、本发明与现有技术相比的优点在于：

71、本发明采用期望最大化与rts平滑算法，实现触觉力的估计，而无需外接触觉力传感器。相比于传统的外接触觉力传感器，本发明方法的可靠性不受外设传感器加工工艺的影响，且过滤过程噪声与量测噪声的干扰、充分利用有效的量测信息，提升力触觉的估计精度。本发明可用于康复机械臂、远程医疗手术机械臂等领域。