【技术实现步骤摘要】
本专利技术涉及一种基于迭代优化的机械臂接触力与环境刚度联合估计方法，适用于无传感器情形下的机械臂控制系统，属于机械臂力触觉反馈与估计领域。
技术介绍
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(·)表示概率。...
【技术保护点】
1.一种基于迭代优化的机械臂接触力与环境刚度联合估计方法，其特征在于：包括以下步骤：
2.根据权利要求1所述的基于迭代优化的机械臂接触力与环境刚度联合估计方法，其特征在于：所述第一步具体实现如下：
3.根据权利要求2所述的基于迭代优化的机械臂接触力与环境刚度联合估计方法，其特征在于：所述第二步具体实现为：
4.根据权利要求3所述的基于迭代优化的机械臂接触力与环境刚度联合估计方法，其特征在于：所述第三步具体实现如下：
【技术特征摘要】
1.一种基于迭代优化的机械臂接触力与环境刚度联合估计方法，其特征在于：包括以下步骤：
2.根据权利要求1所述的基于迭代优化的机械臂接触力与环境刚度联合估计方法，其特征在于：所述第一步具体实现如下：
3.根...
【专利技术属性】
技术研发人员：李文硕，杜晓帆，魏嫣然，余翔，郭雷，
申请(专利权)人：北京航空航天大学杭州创新研究院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。