CN114367990B - 一种基于机理数据混合模型的机械臂触觉外力估计方法 - Google Patents

Abstract

本发明涉及一种基于机理数据混合模型的机械臂触觉外力估计方法，在无接触外力状态下，建立机械臂欧拉‑拉格朗日刚体动力学模型，将实际关节力矩与机理模型计算力矩之差作为未建模残余动态，测量建立包含关节状态和未建模残余动态的数据集；利用高斯过程回归的监督统计学习方法，基于数据集训练未建模残余动态模型补偿项，得到包含机理模型与补偿项的机理数据混合模型；将关节外力建模并作为伪观测量扩维，建立机械臂外力估计模型；设计过程噪声方差随关节状态更新的外力估计方法。本发明实现存在未知耦合未建模动态的机械臂触觉外力估计，适用于需要进行高精度外力估计的多场景多类型力交互式机械臂系统中。

Description

一种基于机理数据混合模型的机械臂触觉外力估计方法

技术领域

本发明属于电数据处理领域，具体涉及一种基于机理数据混合模型的机械臂触觉外力估计方法。

背景技术

随着机器人技术的成熟与发展，机器人开始代替人类进入各种特殊甚至极端恶劣的作业环境，未来战争救援、空间机械臂极端环境抓捕场景下，机器人对环境的交互感知关系到机器人的存活，在这些未知环境、复杂任务下，高精度的交互力信息实时感知与充分利用是机器人智能交互、安全存活的迫切需求。现有的交互力信息感知手段主要是通过外加力传感器实现，然而外加力传感器受到复杂结构、高昂成本、额外安装空间、附加载荷的限制，难以适应复杂场景。采用基于量测和模型信息动态解算的无传感软测量方法，在避免外加力传感器的同时，也受限于精准建模假设。由于机械臂系统具有强耦合、非线性的特性，精确辨识、建模难度大，且本身存在关节摩擦力、关节柔性等难以精确建模的未知未建模残余动态，这些未知未建模残余动态与系统状态耦合，与触觉外力在同一通道，难以实现触觉外力的精确分离与估计。这些特点极大增加了力交互式机械臂的外力感知难度，传统力感知与估计方法难以直接应用。因此，结合现有技术，设计可基于非精准模型的无传感器触觉外力估计方法很重要，具有广泛的应用前景。

目前，对无传感器触觉力估计的研究主要集中于更精确的机械臂系统辨识方面，大多不考虑建模误差，针对与未建模动态耦合情况下的触觉外力有效估计技术研究较少，文献《基于最优激励位姿序列的机械臂负载估计》，利用机器人静力学方法计算负载参数模型，通过关节力矩与位置信息的负载参数离线辨识方法，辨识关节负载，取得了较好的负载力估计效果。文献《在人机协作中基于动量观测和优化算法的全机械臂单点接触信息实时估计》，针对协作力交互型机器人，依据机器人控制与运动状态信息，利用优化算法有界搜索，实现无传感器估计接触力发生位置。文献《机械臂外力估计与控制方法研究及应用》，提出一种基于H_∞滤波的机械臂外力估计方法，保证外力估计的鲁棒性。但上述三种方法基于简化后的机械臂模型，忽略了许多原系统中的与外力耦合的未建模非线性动态，难以保证估计精度。中国发明专利CN201410112717.5中提出了一种基于H_∞滤波的机械臂外力估计方法，针对实际机械臂系统外力估计问题，采用模拟低通滤波器对动力学方程中信号进行滤波，将外力扩维后基于H_∞滤波估计外力，无需加速度信号的测量，然而此方法没有考虑实际存在的模型不确定性，影响外力估计精度。中国发明专利CN201710834869.X中提出了一种外力估计方法及装置，利用多层感知机对建模误差进行建模补偿，实现无外加力传感器的外力估计，通过引入卡尔曼滤波器，实现在观测噪声和模型误差存在的情况下具有一定鲁棒性，但未考虑对误差建模的置信度，且利用多层感知机建模方法不具有可解释性，建模参数化模型需要的数据量非常大，不适用于实际工程应用。

综上所述，当力交互机械臂系统存在与外力耦合的未知未建模动态时，现有方法缺乏可抗耦合干扰的高精度触觉力分离估计方法，亟需攻克基于数据增强混合模型的无传感触觉力估计方法。

发明内容

本发明解决的技术问题是：在未知未建模残余动态与触觉外力耦合下的机械臂触觉力估计问题，为克服现有技术的不足，本发明提供一种基于机理数据混合模型的机械臂触觉外力估计方法，实现了对机理动力学模型残余未建模动态的小样本建模与补偿，采用一套触觉外力估计策略实时估计未知外力，无需增加额外的触觉传感器，且可充分利用建模的置信度信息，对模型进行实时修正，提升触觉外力估计过程的自主性、精确性及抗干扰能力。

本发明的技术解决方案为：一种基于机理数据混合模型的机械臂触觉外力估计方法，具体实现步骤如下：

第一步，在无接触外力状态下，建立作为机理模型的机械臂欧拉-拉格朗日刚体动力学模型，通过测量关节电流值，得到实际关节力矩，将所述实际关节力矩与机理模型计算的力矩之差作为未建模残余动态，以关节状态作为模型输入、以所述未建模残余动态的测量值作为模型输出，建立模型训练数据集；

第二步，将第一步中的所述机理模型转换为动量形式，基于第一步建立的所述模型训练数据集，利用高斯过程回归的监督统计学习方法，训练针对所述未建模残余动态的补偿项模型，模型输出为对未建模残余动态的预测均值和方差，得到包含机理模型与基于未建模残余动态数据的补偿项的机理数据混合模型；

第三步，将关节外力建模成随机游走形式，并作为伪观测量扩维动量组成新的状态变量，建立机械臂外力估计系统模型；

第四步，设计带有过程噪声实时修正的Kalman滤波方法估计触觉外力，所述过程噪声的实时方差信息来源于第二步中的所述补偿项，为所述补偿项模型的预测输出方差，随机械臂外力估计系统的状态变量更新，完成基于机理数据混合模型的机械臂触觉外力估计方法。

进一步地，所述第一步的具体步骤如下：

(1) 建立n连杆标准刚体机械臂动力学模型作为机理模型，在机械臂关节空间，将机械臂状态变量与关节力矩的关系用拉格朗日方程的形式表示：

其中，q、

、

分别为关节角度、角速度和角加速度，表示机械臂状态变量；M(q)为惯性阵，

表示哥氏力和离心力项，G(q)表示重力项，τ _EL 表示机理模型计算的关节力矩，n为大于或等于1的整数；

(2) 实际关节力矩通过测量关节电流值获得：

τ _m =K _a ·I _m

其中，K _a 表示电流系数，I _m 表示电机输出电流测量值，τ _m 为实际关节力矩；

未建模残余动态测量值计算公式如下：

ε=τ _m -τ _EL

其中，τ _m 为实际关节力矩，ε为机理建模的实际关节力矩值与机理模型计算的关节力矩的误差；经过数据分析，选取关节角度、角速度和角加速度q、

、

作为回归特征，ε作为回归真值，建立模型回归的模型训练数据集：

，D为训练集符号。

进一步地，所述第二步的具体步骤如下：

(1) 机械臂的状态变量以动量形式表征如下：

其中，p表示关节动量；在有接触外力状态下，求导得到动量形式表征的机械臂动力学机理模型如下：

其中，

表示动量的导数，

中的上标T表示矩阵的转置，τ _ext 表示有接触外力状态下，接触外力作用在关节处的外力矩；

(2) 待训练的未建模残余动态模型表示如下：

其中，

表示未建模残余动态模型的函数，W _r 表示未建模残余动态模型的随机噪声向量，其每个分量均为零均值高斯白噪声，形式为：

其中，w _r,i 表示随机噪声向量W _r 的第i个分量，

表示服从均值为零，方差为 σ _r,i ²的高斯分布；

将待训练的未建模残余动态在关节状态空间视作连续，符合多元联合高斯分布，基于贝叶斯推断的函数模型后验概率函数为：

其中，p为概率分布符号；θ为超参数，包含均值函数、核函数、似然函数、推断方法、稀疏近似的参数值和函数类型；Z为回归量符号，表示关节角度、角速度和角加速度

组成的向量；p(Φ|Z,ε,θ)表示未建模残余动态模型的后验概率函数，p(ε|Z,Φ,θ)表示未建模残余动态模型的似然函数，p(Φ|θ)表示未建模残余动态模型的超参数先验函数，p(ε|Z,θ)表示未建模残余动态模型的边际似然函数；

(3) 在模型训练数据集D的条件下，基于最大后验概率推断优化未建模残余动态模型的超参数，从而训练未建模残余动态模型；

未建模残余动态的边际似然函数形式为：

其中，π表示圆周率，N表示数据集中数据对的数量，K表示数据集中数据对的协方差矩阵，e表示自然常数；

基于最大后验概率函数推断优化未建模残余动态模型的超参数，从而训练未建模残余动态模型的优化函数，选取未建模残余动态的负对数边际似然函数，形式如下：

其中，ln表示自然对数符号；优化函数采用共轭梯度法极小化求解，从而得到未建模残余动态模型的超参数；训练得到的未建模残余动态模型是一个随机过程，由模型训练数据集D=<Z,ε>、超参数θ组成，模型输出预测均值作为函数

的输出和方差σ ²作为过程噪声的方差，上标T表示转置符号；

将未建模残余动态模型作为补偿项，加入到机理模型中，组成机理数据混合模型如下：

进一步表示成连续线性机械臂动量系统形式：

其中，u表示机械臂动量系统状态方程输入，形式如下：

离散化机械臂动量系统状态方程为：

p _k =p _k-1+ΔT·u _k +ΔT·τ _ext,k +W _r,k

其中，p _k 和p _k-1分别表示第k、k-1时刻的动量，ΔT表示离散化间隔采样时间，τ _ext,k 表示第k时刻机械臂动量系统所受触觉外力作用在关节上的外力矩，W _r,k 表示第k时刻机械臂动量系统的过程噪声，u _k 表示第k时刻机械臂动量系统状态方程输入，计算如下：

其中，

分别表示第k时刻的关节角度、角速度、角加速度，τ _m,k 表示第k时刻实际关节力矩值。

进一步地，所述第三步中，以随机游走形式建模关节外力如下：

τ _ext,k =τ _ext,k-1 +W _ext,k

其中，τ _ext,k-1 表示第k-1时刻关节外力，W _ext,k 表示第k时刻关节外力系统过程噪声，为零均值高斯白噪声，方差为σ _ext ²；

建立机械臂外力估计系统模型如下：

x _k =A _k x _k-1 +B _k u _k +W _k

y _k =C _k x _k +V _k

其中，

表示第k时刻外力估计系统的系统状态，为关节动量和关节外力扩维后的状态变量；A _k 表示第k时刻状态矩阵，B _k 表示第k时刻输入矩阵，

表示第k时刻总过程噪声，为动量系统和关节外力系统的过程噪声扩维组成；y _k 表示第k时刻测量值，C _k 表示第k时刻输出矩阵，V _k 表示第k时刻量测噪声。

进一步地，所述第四步，基于Kalman滤波方法设计带有过程噪声实时修正的触觉外力估计方法为：

基于所述第三步中建立的外力估计系统模型，机械臂触觉外力估计方程为如下6步：

(1) 状态预测：

其中，x _k|k-1表示由k-1时刻对k时刻外力估计系统状态的预测，

表示k-1时刻外力估计系统状态的估计值；

(2) 协方差预测：

其中，P _k|k-1表示由k-1时刻对k时刻协方差的预测，

表示k-1时刻协方差的估计值，

表示状态矩阵的转置；过程噪声W _k 包含W _r,k 、W _ext,k 两个分量，方差修正信息分别如下：

，

其中，σ _ext ²为关节外力系统中过程噪声的时不变方差。即，总过程噪声W _k 包含的两个分量中，W _r,k 具有时变方差σ _r,k ²，来源于第二步中的方差σ _r,i ²， W _ext,k 具有时不变方差σ _ext ²；

(3) 最优Kalman增益：

其中，K _k 表示k时刻的Kalman增益，

表示输出矩阵的转置；

(4) 测量更新状态估计：

(5) 更新协方差估计：

其中，I表示单位矩阵；

(6) 机械臂关节外力估计输出：

其中，

表示关节外力估计值，H表示关节外力输出矩阵。

本发明与现有技术相比的优点在于：本发明涉及的一种基于机理数据混合模型的机械臂触觉外力估计方法，针对现有方法在含有与触觉外力耦合的未知未建模残余动态的情况下，缺乏可抗耦合干扰的高精度触觉力分离估计能力的缺点，设计了对机理动力学模型残余未建模动态的小样本建模与补偿策略，实现了机械臂系统高精度建模，使其能够在不同作用场景、存在多种未知建模误差及多源干扰动态下均可以保证精准建模；再者，设计了一套触觉外力估计策略实时估计未知外力，无需增加额外的触觉传感器，且可充分利用建模的置信度信息，对模型进行实时修正，提升触觉外力估计过程的自主性、精确性及抗干扰能力，适用于多场景多类型的力交互式机械臂的高精度外力估计中。

附图说明

图1为本发明的一种基于机理数据混合模型的机械臂触觉外力估计方法的设计流程图；

图2为本发明的一种基于机理数据混合模型的机械臂触觉外力估计方法的框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明涉及一种基于机理数据混合模型的机械臂触觉外力估计方法。第一步，在无接触外力状态下建立机械臂刚体动力学机理模型，测量关节电机电流得到实际关节力矩，所述实际关节力矩与机理模型计算的力矩残差作为未建模残余动态，以关节状态作为模型输入、以未建模残余动态的测量值作为模型输出，建立模型训练数据集；第二步，将第一步中的机理模型转换为动量形式，基于第一步建立的模型训练数据集，利用高斯过程回归的监督统计学习方法，训练针对所述未建模残余动态的补偿项模型，模型输出为对所述未建模残余动态的预测均值和方差，得到包含机理模型与基于未建模残余动态的补偿项的机理数据混合模型；第三步，将关节外力建模成随机游走形式，并作为伪观测量扩维动量组成新的状态变量，建立机械臂外力估计系统模型；第四步，设计带有过程噪声实时修正的Kalman滤波方法估计触觉外力，所述过程噪声的实时方差信息来源于第二步中的所述补偿项，为补偿项模型的预测输出方差，随机械臂外力估计系统状态变量更新，完成基于机理数据混合模型的机械臂触觉外力估计方法。本发明设计对机理动力学模型残余未建模动态的小样本建模与补偿策略，能够在不同作用场景、存在多种未知建模误差及多源干扰动态下均可以保证精准建模，实现存在未知耦合未建模动态的机械臂触觉外力估计，且可充分利用建模的置信度信息，对模型进行实时修正，具有高精度、易于工程实现、具有可解释性及抗干扰能力的特点，适用于需要进行高精度外力估计的多场景多类型力交互式机械臂系统中。

以单连杆机械臂为例，具体实施步骤如下：

第一步，在无接触外力状态下，基于欧拉-拉格朗日刚体动力学计算方法，建立单连杆标准刚体机械臂的动力学模型作为机理模型，在机械臂关节空间，将机械臂相关状态变量与关节力矩的关系用拉格朗日方程的形式表示：

其中，q、

分别为关节角度和角加速度，另外，

表示关节角速度，q、

、

表示机械臂系统状态变量；m、l、g分别表示质量、杆长和重力加速度，取m=1kg、l=0.5m、g=9.8m/s²；M(q)为惯性阵，在本例中M(q)=ml ²；

表示哥氏力和离心力项，在本例中为零；G(q)表示重力项，本例为G(q)=lmgsin(q)，τ _EL 表示机理模型计算的关节力矩。

通过测量关节电流值，得到实际关节力矩，将实际关节力矩与机理模型计算的关节力矩之差作为未建模残余动态，以关节状态作为模型输入、以未建模残余动态测量值作为模型输出，建立模型训练数据集。

实际关节力矩通过测量关节电流值获得：

τ _m =K _a ·I _m

其中，K _a 表示电流系数，取-1.78N·m/A，I _m 表示电机输出电流测量值，τ _m 为实际关节力矩。

未建模残余动态测量值计算公式如下：

ε=τ _m -τ _EL

其中，ε为机理模型的关节力矩计算值与实际关节力矩的误差，在回归建模未建模残余动态中作为测量真值；经过数据分析，选取关节角度、角速度、角加速度q、

、

作为回归特征，ε作为回归真值，建立模型回归的模型训练数据集：

，D为训练集符号。

第二步，将第一步中的机理模型转换为动量形式，机械臂状态以动量形式表征如下：

其中，p表示关节动量。在有接触外力状态下，求导得到动量形式表征的机械臂动力学机理模型如下：

其中，

表示动量的导数，τ _ext 表示有接触外力状态下，接触外力作用在关节处的外力矩。

待训练的未建模残余动态模型表示如下：

其中，

表示未建模残余动态模型的函数，W _r 表示未建模残余动态模型的随机噪声向量，其每个分量均为零均值高斯白噪声，形式为：

其中，w _r,i 表示随机噪声向量W _r 的第i个分量，

表示服从均值为零，方差为σ _r,i ²的高斯分布。

将待训练的未建模残余动态在关节状态空间视作连续，符合多元联合高斯分布，基于贝叶斯推断的函数模型后验概率函数为：

其中，p为概率分布符号；θ为超参数，包含均值函数、核函数、似然函数、推断方法、稀疏近似的参数值和函数类型；Z为回归量符号，表示关节状态

组成的向量；p(Φ|Z,ε,θ)表示未建模残余动态模型的后验概率函数，p(ε|Z,Φ,θ)表示未建模残余动态模型的似然函数，p(Φ|θ)表示未建模残余动态模型的超参数先验函数，p(ε|Z,θ)表示未建模残余动态模型的边际似然函数。

基于第一步建立的模型训练数据集，利用高斯过程回归建模方法，基于最大后验概率推断优化未建模残余动态模型的超参数，从而训练未建模残余动态模型。

未建模残余动态的边际似然函数形式为：

其中，π表示圆周率，N表示数据集中数据对的数量，K表示数据集中数据对的协方差矩阵，e表示自然常数。

基于最大后验概率推断优化未建模残余动态模型的超参数训练未建模残余动态模型的优化函数，选取未建模残余动态的负对数边际似然函数形式如下：

其中，ln表示自然对数符号。优化函数采用共轭梯度法极小化求解，从而得到未建模残余动态模型的超参数。训练得到的未建模残余动态模型是一个随机过程，由数据集D=<Z,ε>、超参数θ组成，模型输出预测均值作为未建模残余动态模型的函数

的输出和方差σ ²作为过程噪声的方差，上标T表示向量的转置。

本例训练过程中，高斯过程回归训练模型各参数设置为：均值函数选取零均值函数，核函数选取谱混合核函数，似然函数为二次指数自相关函数，推断方法为高斯似然推断，稀疏近似选取FIFC近似。

将未建模残余动态模型作为补偿项，加入到机理模型中，组成机理数据混合模型如下：

进一步表示成连续线性机械臂动量系统形式：

其中，u表示机械臂动量系统状态方程输入，形式如下：

离散化机械臂动量系统状态方程为：

p _k =p _k-1+ΔT·u _k +ΔT·τ _ext,k +W _r,k

其中，p _k 和p _k-1分别表示第k、k-1时刻的动量，ΔT表示离散化间隔采样时间，取为0.01s，τ _ext,k 表示第k时刻机械臂动量系统所受触觉外力作用在关节上的外力矩，W _r,k 表示第k时刻机械臂动量系统的过程噪声，u _k 表示第k时刻机械臂动量系统状态方程输入，计算如下：

其中，

分别表示第k时刻的关节角度、关节角速度、关节角加速度，τ _m,k 表示第k时刻实际关节力矩。

第三步，以随机游走形式建模关节外力系统，具体步骤如下：

τ _ext,k =τ _ext,k-1 +W _ext,k

其中，τ _ext,k-1 表示第k-1时刻关节外力，W _ext,k 表示第k时刻关节外力系统过程噪声，为零均值高斯白噪声，方差为σ _ext ²。

建立机械臂外力估计系统模型如下：

x _k =A _k x _k-1 +B _k u _k +W _k

y _k =C _k x _k +V _k

其中，

表示第k时刻外力估计系统的系统状态，为关节动量和关节外力扩维后的状态变量；A _k 表示第k时刻状态矩阵，B _k 表示第k时刻输入矩阵，

表示第k时刻总过程噪声，为动量系统和关节外力系统的过程噪声扩维组成；y _k 表示第k时刻测量值，C _k 表示第k时刻输出矩阵，V _k 表示第k时刻量测噪声，取为零均值高斯白噪声，取

。

各矩阵具体为：

，

，

第四步，基于Kalman滤波方法设计带有过程噪声实时修正的触觉外力估计方法：

基于第三步中建立的外力估计系统模型，机械臂触觉外力估计方程为：

状态预测：

其中，x _k|k-1表示由k-1时刻对k时刻外力估计系统状态的预测，

表示k-1时刻外力估计系统状态的估计值。

协方差预测：

其中，P _k|k-1表示由k-1时刻对k时刻协方差的预测

表示k-1时刻协方差的估计值，

表示状态矩阵的转置；过程噪声W _k 包含W _r,k 、W _ext,k 两个分量，方差修正信息分别如下：

，

其中，σ _ext ²为关节外力系统中过程噪声的时不变方差。即，总过程噪声W _k 包含的两个分量中，W _r,k 具有时变方差σ _r,k ²，随外力估计系统的状态变量更新，W _ext,k 具有时不变方差σ _ext ²，取为0.001。

最优Kalman增益：

其中，K _k 表示k时刻的Kalman增益，

表示输出矩阵的转置。

测量更新状态估计：

更新协方差估计：

其中，I表示单位矩阵。

机械臂关节外力估计输出：

其中，

表示关节外力估计值，H表示关节外力输出矩阵。

此外，当连杆数n为大于1的整数时，在第一步中同样采用欧拉-拉格朗日法计算机械臂的动力学模型作为机理模型，与本例的区别仅在于：第一步中的惯性矩阵M(q)、哥氏力和离心力项矩阵

和重力项矩阵G(q)不同，所述不同与n相关；其他步骤并无区别。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于机理数据混合模型的机械臂触觉外力估计方法，其特征在于，包括以下步骤：

第一步，在无接触外力状态下，建立作为机理模型的机械臂欧拉-拉格朗日刚体动力学模型，通过测量关节电流值，得到实际关节力矩，将所述实际关节力矩与机理模型计算的关节力矩之差作为未建模残余动态，以关节状态作为模型输入、以所述未建模残余动态的测量值作为模型输出，建立模型训练数据集；

第二步，将第一步中的所述机理模型转换为动量形式，基于第一步建立的所述模型训练数据集，利用高斯过程回归的监督统计学习方法，训练针对所述未建模残余动态的补偿项模型，模型输出为对未建模残余动态的预测均值和方差，得到包含机理模型与基于未建模残余动态的数据的补偿项的机理数据混合模型；

第三步，将关节外力建模成随机游走形式，并作为伪观测量扩维动量组成新的状态变量，建立机械臂外力估计系统模型；

第四步，设计带有过程噪声实时修正的Kalman滤波方法估计触觉外力，所述过程噪声的实时方差信息来源于第二步中的所述补偿项，为所述补偿项模型的预测输出方差，随机械臂外力估计系统的状态变量更新，完成基于机理数据混合模型的机械臂触觉外力估计方法。

2.根据权利要求1所述的一种基于机理数据混合模型的机械臂触觉外力估计方法，其特征在于：所述第一步的具体步骤如下：

(1) 建立n连杆标准刚体机械臂动力学模型作为机理模型，在机械臂关节空间，将机械臂状态变量与关节力矩的关系用拉格朗日方程的形式表示：

其中，q、

、

分别为关节角度、角速度和角加速度，表示机械臂状态变量；M(q)为惯性阵，

表示哥氏力和离心力项，G(q)表示重力项，τ _EL 表示机理模型计算的关节力矩，n为大于或等于1的整数；

(2) 实际关节力矩通过测量关节电流值获得：

τ _m =K _a ·I _m

其中，K _a 表示电流系数，I _m 表示电机输出电流测量值，τ _m 为实际关节力矩；

未建模残余动态测量值计算公式如下：

ε=τ _m -τ _EL

其中，τ _m 为实际关节力矩，ε为机理建模的实际关节力矩与机理模型计算的关节力矩的误差，在回归建模未建模残余动态中作为测量真值；经过数据分析，选取关节角度、角速度和角加速度q、

、

作为回归特征，ε作为回归真值，建立模型回归的模型训练数据集：

，D为训练集符号。

3.根据权利要求2所述的一种基于机理数据混合模型的机械臂触觉外力估计方法，其特征在于：所述第二步的具体步骤如下：

(1) 机械臂的状态变量以动量形式表征如下：

其中，p表示关节动量；在有接触外力状态下，求导得到动量形式表征的机械臂动力学机理模型如下：

其中，

表示动量的导数，

中的上标T表示矩阵的转置，τ _ext 表示有接触外力状态下，接触外力作用在关节处的外力矩；

(2) 待训练的未建模残余动态模型表示如下：

其中，

表示未建模残余动态模型的函数，W _r 表示未建模残余动态模型的随机噪声向量，其每个分量均为零均值高斯白噪声，形式为：

其中，w _r,i 表示随机噪声向量W _r 的第i个分量，

表示服从均值为零，方差为σ _r,i ²的高斯分布；

将待训练的未建模残余动态在关节状态空间视作连续，符合多元联合高斯分布，基于贝叶斯推断的函数模型的后验概率函数为：

其中，p(·)为概率分布；θ为超参数，包含均值函数、核函数、似然函数、推断方法、稀疏近似的参数值和函数类型；Z为回归量符号，表示关节角度、角速度和角加速度

组成的向量；p(Φ|Z,ε,θ)表示未建模残余动态模型的后验概率函数，p(ε|Z,Φ,θ)表示未建模残余动态模型的似然函数，p(Φ|θ)表示未建模残余动态模型的超参数先验函数，p(ε|Z,θ)表示未建模残余动态模型的边际似然函数；

(3) 在模型训练数据集D的条件下，基于最大后验概率推断优化未建模残余动态模型的超参数，从而训练未建模残余动态模型；

未建模残余动态模型的边际似然函数形式为：

其中，π表示圆周率，N表示数据集中数据对的数量，K表示数据集中数据对的协方差矩阵，e表示自然常数；

基于最大后验概率函数推断优化未建模残余动态模型的超参数，从而训练未建模残余动态模型的优化函数，选取未建模残余动态模型的负对数边际似然函数，形式如下：

其中，ln表示自然对数符号；优化函数采用共轭梯度法极小化求解，从而得到未建模残余动态模型的超参数；训练得到的未建模残余动态模型是一个随机过程，由模型训练数据集D=<Z,ε>、超参数θ组成，模型输出预测均值作为未建模残余动态模型的函数

的输出和方差σ ²作为过程噪声的方差，上标T表示转置符号；

将未建模残余动态模型作为补偿项，加入到机理模型中，组成机理数据混合模型如下：

进一步表示成连续线性机械臂动量系统形式：

其中，u表示机械臂动量系统状态方程输入，形式如下：

离散化机械臂动量系统状态方程为：

p _k =p _k-1 +ΔT·u _k +ΔT·τ _ext,k +W _r,k

其中，p _k 和p _k-1 分别表示第k、k-1时刻的动量，ΔT表示离散化间隔采样时间，τ _ext,k 表示第k时刻机械臂动量系统所受触觉外力作用在关节上的外力矩，W _r,k 表示第k时刻机械臂动量系统的过程噪声，u _k 表示第k时刻机械臂动量系统状态方程输入，计算如下：

其中，

分别表示第k时刻的关节角度、关节角速度、关节角加速度，τ _m,k 表示第k时刻实际关节力矩值。

4.根据权利要求3所述的一种基于机理数据混合模型的机械臂触觉外力估计方法，其特征在于：所述第三步中，以随机游走形式建模关节外力系统如下：

τ _ext,k =τ _ext,k-1 +W _ext,k

其中，τ _ext,k-1 表示第k-1时刻关节外力，W _ext,k 表示第k时刻关节外力系统过程噪声，为零均值高斯白噪声，方差为σ _ext ²；

建立机械臂外力估计系统模型如下：

x _k =A _k x _k-1 +B _k u _k +W _k

y _k =C _k x _k +V _k

其中，

表示第k时刻外力估计系统的系统状态，为关节动量和关节外力扩维后的状态变量；A _k 表示第k时刻状态矩阵，B _k 表示第k时刻输入矩阵，

表示第k时刻总过程噪声，为机械臂动量系统和关节外力系统的过程噪声扩维组成；y _k 表示第k时刻测量值，C _k 表示第k时刻输出矩阵，V _k 表示第k时刻量测噪声。

5.根据权利要求4所述的一种基于机理数据混合模型的机械臂触觉外力估计方法，其特征在于：所述第四步，基于Kalman滤波方法设计带有过程噪声实时修正的触觉外力估计方法为：

基于所述第三步中建立的所述外力估计系统模型，机械臂触觉外力估计方程为如下6步：

(1) 状态预测：

其中，x _k|k-1表示由k-1时刻对k时刻外力估计系统状态的预测，

表示k-1时刻外力估计系统状态的估计值；

(2) 协方差预测：

其中，P _k|k-1表示由k-1时刻对k时刻协方差的预测，

表示k-1时刻协方差的估计值，

表示状态矩阵的转置；过程噪声W _k 包含W _r,k 、W _ext,k 两个分量，方差修正信息分别如下：

，

其中，σ _ext ²为关节外力系统中过程噪声的时不变方差，即，总过程噪声W _k 包含的两个分量中，W _r,k 具有时变方差σ _r,k ²，来源于第二步中的方差σ _r,i ²，W _ext,k 具有时不变方差σ _ext ²；

(3) 最优Kalman增益：

其中，K _k 表示k时刻的Kalman增益，

表示输出矩阵的转置；

(4) 测量更新状态估计：

(5) 更新协方差估计：

其中，I表示单位矩阵；

(6) 机械臂关节外力估计输出：

其中，

表示关节外力估计值，H表示关节外力输出矩阵。

Images