一种基于动态臂力估计的人机协作方法

1.本发明属于人机协同设备智能控制的技术领域，具体涉及一种基于动态臂力估计的人机协作方法。


背景技术：

2.对于工业应用中的物理人机交互场景来说，基于表面肌电信号(semg)的意图识别相对于视觉，力传感器等意图识别方式，具有不受应用场景的限制、高柔性、高适应性、低系统复杂性的优点，因此受到研究者们的广泛关注。
3.目前，对于建立semg与机器人动态臂力的关系，包括通过系统辨识的方式建立了每个通道semg与输出力的模型、基于角度的肌电幅值校准和并行级联辨识相结合的方式用于估计动态收缩时肘关节在腕部产生的力、利用长短时记忆网络(lstm)建立了前后三角肌与水平力之间的映射关系。
4.但是，当前的方法只关注单个方向或单个关节的力估计，应用到复杂人机协作场景存在很多挑战。影响力估计效果的瓶颈问题在于semg幅值估计方法的稳定性，臂力估计过程中的semg延时问题，多个通道semg融合问题以及semg特征的时序关系的捕捉问题。
5.目前，虽然存在利用并行级联模型捕捉肌电延时、利用广义典型相关分析建立了一种实时特征提取与融合模型、利用线性回归方法对多个通道semg信号进行加权并使用递归神经网络捕捉融合特征间的时间信息三类技术方案。然而，到目前为止没有一种方案能够同时解决semg延时、多个通道semg融合以及semg特征的时序关系捕捉问题的方法。
6.此外，现有的semg幅值估计方法只是利用经验法确定了幅值估计的时间窗口长度，而基于经验的方法通常很难保证技术方案的再现性；另一方面，目前标准化的幅值估计方法，是基于非因果迭代学习的幅值估计方法，非因果迭代学习意味着要利用未来的肌电数据估计当前时刻的幅值，因而只适合于离线分析，无法适应在线实时人机协作过程。


技术实现要素：

7.为了克服现有技术存在的一个或者多个缺陷与不足，本发明的目的在于提供一种基于动态臂力估计的人机协作方法，用于在实时的人机协作下同时解决semg延时、多个通道semg融合以及semg特征的时序关系捕捉问题。
8.为了达到上述目的，本发明采用以下的技术方案。
9.一种基于动态臂力估计的人机协作方法，包括步骤如下：
10.采集人机协作中操作员在执行设定动作时手臂多个位置上的表面肌电信号semg、角度信号、对协作机械臂施加的力度信号，将semg、角度信号、力度信号输入数据信号处理系统进行后续的处理；
11.对多个semg进行自适应幅值提取，得到相应多个通道的semg幅值；然后对多个通道的semg幅值进行自适应平滑处理，得到平滑后的多个通道的semg幅值；
12.将平滑后的多个通道的semg幅值作为输入，通过融合神经网络对多个通道的semg
幅值依此进行延时补偿、通道融合、时序捕捉，得到对应多个通道的semg幅值的融合特征；然后将融合特征输入估计神经网络，获得融合特征与操作员臂力的映射关系，实现从多个通道的semg幅值对操作员臂力的估计；
13.数据信号处理系统根据对操作员臂力的估计进行协作控制，得到与操作员设定动作相配合的协作，然后驱动协作机械臂做出协作动作。
14.优选地，对多个semg进行自适应幅值提取的步骤如下：
15.构建semg幅值的提取窗口，得到提取窗口的长度的迭代公式；根据基于信息熵的迭代收敛准则，对提取窗口的长度进行迭代，在满足迭代收敛准则时提取窗口的长度为最优；
16.获得最优的提取窗口的长度后，使用最优的提取窗口对semg幅值进行自适应提取，得到自适应的多个通道的semg幅值。
17.优选地，提取窗口的长度的迭代公式为：
[0018][0019]
其中，mk表示提取窗口的长度，α为整流器系数,v为重线性化系数，ωk为k时刻自适应的semg幅值，ak为semg的幅值的一阶导数、bk为semg的幅值的二阶导数，γ(
·
)表示欧拉伽马函数。
[0020]
进一步地，基于信息熵的迭代收敛准则为：
[0021][0022]
其中，h(sk)表示基于信息熵的迭代收敛准则，)表示基于信息熵的迭代收敛准则，sk表示semg幅值sk在当前迭代次数下提取窗口长度mk的平均值，p(sk)表示迭代次数下具有mk阶自由度的卡方分布概率密度函数χ2(
·
)。
[0023]
优选地，进行自适应平滑处理的步骤如下：
[0024]
采用分形维数d评估自适应幅值提取后多个通道的semg幅值的包络的跳动程度；
[0025]
构建指数衰减函数ε与分形维数d的关系，根据粒子群优化算法获取指数衰减函数ε中最优的衰减因子a；
[0026]
采用包含最优的衰减因子a的指数衰减函数ε对自适应幅值提取后多个通道的semg幅值的包络进行平滑。
[0027]
进一步地，分形维数d的计算如下式所示：
[0028][0029]
而：
[0030][0031][0032]
其中，n(δ)、n(2δ)分别表示用宽度为δ、2δ的网格覆盖semg幅值包络时所需的网格数，l表示在设定的采样周期内所采集的semg信号的样本量，ωi表示在设定的采样周期内的第i时刻的semg幅值包络。
[0033]
再进一步地，指数衰减函数ε与分形维数d关系如下式所示：
[0034]
ε＝e-ad
[0035]
对semg幅值的包络进行平滑如下式所示：
[0036][0037]
其中，为第k时刻的semg幅值特征。
[0038]
优选地，融合神经网络包括依次连接的延时补偿模块、通道融合模块、时序捕捉模块；融合神经网络以平滑后多个通道的semg幅值作为输入，输出为对应的融合特征；
[0039]
延时补偿模块用于对经过自适应平滑处理后的多个通道的semg幅值进行延时补偿；
[0040]
通道融合模块用于将经过延时补偿的多个通道的semg幅值进行融合，获得相应的融合特征；
[0041]
时序捕捉模块用于分析融合特征之间的时序关系，输出具有时序关系的融合特征。
[0042]
进一步地，估计神经网络包括相互连接的长短时记忆网络与注意力机制网络；
[0043]
长短时记忆网络以具有时序关系的融合特征作为输入，长短时记忆网络输出具有强化时序关系的融合特征；
[0044]
注意力机制网络以强化时序关系的融合特征作为输入，输出为估计的臂力，从而将多个通道的semg幅值映射到操作员的臂力上，实现臂力估计。
[0045]
优选地，协作控制具体采用pd速度控制、分段速度控制两种方式计算位移量，pd速度控制、分段速度控制分别如下式所示：
[0046]
[0047][0048]
其中，f
x
、fy分别为臂力的水平力、垂直力，表示水平力的导数，k
p
、kd分别为pd控制的比例调节参数、微分调节参数，δx、δy分别为机器人在水平方向、垂直方向的位移。
[0049]
本发明技术方案与现有技术相比，具有如下有益效果：
[0050]
本发明相比现有的动态臂力估计协作方法，通过自适应幅值提取与基于分形维数的自适应平滑相结合的方式，实现了基于信号自身特性的特征的标准化提取，保证了提取的规范化、稳定性；兼顾臂力估计过程中的延时、多通道融合、时序关系捕捉的难点，利用智能的算法提高了动态臂力估计的精度，利用长短时记忆网络与注意力机制实现从semg幅值到动态臂力的映射，建立了定制化的协作控制方法，将所估计的动态臂力转化为机器人的协作行为，具有延时小、臂力估计精度高的优点。
附图说明
[0051]
图1为本发明其中一种基于动态臂力估计的人机协作方法的流程示意图；
[0052]
图2为图1中进行数据采集时所使用的平台设施示意图。
具体实施方式
[0053]
为了使本发明的目的、技术方案及其优点更加清楚明白，以下结合附图及其实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0054]
实施例
[0055]
如图1所示,本实施例的一种基于动态臂力估计的人机协作方法，包括步骤如下：
[0056]
s1、结合图2所示的平台进行数据采集，图2所示的平台可整体视为一个数据信号处理系统；过程包括：
[0057]
s11、操作员在其激活肌肉贴上双电极肌电传感器采集表面肌电信号，激活肌肉包括手臂的前三角肌、后三角肌、肱二头肌、肱三头肌，在肩关节贴上陀螺仪采集肩关节转动的角度信号，握住协作机械臂末端的六维力传感器采集手臂输出的力度信号；
[0058]
操作员执行预设的相关动作，表面肌电信号和角度信号通过蓝牙通信的方式传送到工控机和实时控制系统，得到semg信号、角度信号；力度信号通过倍幅模块放大幅值后，再通过控制柜与工控机和实时控制系统进行数据交互；在其他实施方式中，具体采集的肌肉位置、信号的通道数目均可以进行灵活设置；
[0059]
s12、将semg信号、角度信号、力度信号集中在工控机中转换为数据进行存储，以便进行后续的处理，后续的处理过程也都在工控机内完成；
[0060]
s2、对采集到的semg信号分别进行自适应幅值提取，得到分别对应前三角肌、后三
角肌、肱二头肌、肱三头肌的四个通道各自的semg幅值；过程包括：
[0061]
s21、构建semg幅值的提取窗口，建立semg幅值与提取窗口的对应关系；具体为：
[0062]
semg幅值特征包括平均绝对值(mav)和均方根值(rms)，对于因果形式的semg幅值特征，其计算如式(1)所示：
[0063][0064]
其中，mk表示提取窗口的长度，ω
k-j
为k-j时刻的semg幅值，α是采集信号时的整流器系数,v是采集信号时的重线性化系数，特别地，当α＝1且v＝1时，为第k时刻的mav估计幅值，当α＝1且v＝2时，为第k时刻的rms估计幅值；
[0065]
由式(1)可以看出，semg的估计幅值取决于提取窗口的长度mk；
[0066]
将式(1)中的ω
k-j
在j＝k的邻域内进行二阶泰勒展开，如式(2)所示：
[0067][0068]
其中，dk表示幅值曲线在k时刻的一阶导数，d
′k表示幅值曲线在k时刻的二阶导数；
[0069]
如式(3)所示，使用牛顿向后微分公式进行近似计算，得到dk、d
′k：
[0070][0071]
如式(4)所示的近似公式：
[0072][0073]
使用式(4)对式(2)进行简化，得到简化后的式(2)可以重写为式(5)：
[0074][0075]
再次使用式(4)对式(5)进行简化，并忽略其中的高阶项再次使用式(4)对式(5)进行简化，并忽略其中的高阶项得到式(6)：
[0076]
[0077]
根据式(6)可将偏差项表示为：
[0078][0079]
由此，定义semg的幅值包络的一阶导数ak如式(8)所示、semg的幅值包络的二阶导数bk如式(9)所示：
[0080][0081][0082]
此时式(7)可以进一步表示为：
[0083]bk
＝akmk bkm
k2
ꢀꢀꢀꢀꢀꢀ
(10)
[0084]
而进行估计幅值时，相应的方差项如式(11)所示：
[0085][0086]
其中，f(α,v)表示为：
[0087][0088]
其中，γ(
·
)表示欧拉伽马函数；
[0089]
根据从式(1)到式(12)的推导，可见semg的幅值估计误差的平方，可以表示为式(13)：
[0090][0091]
从式(13)可获得，semg的幅值估计误差的平方e
k2
关于窗口长度mk的导数如式(14)所示：
[0092][0093]
令时，才可获得最优的提取窗口的长度mk；
[0094]
采用以semg的幅值包络的一阶导数ak占主导(式(8))、semg的幅值包络的二阶导数bk占主导(式(9))，这两种特定的情况来获得简化的近似解，此时最优的提取窗口的长度mk的迭代公式如式(15)所示：
[0095][0096]
s21、设定基于信息熵的迭代收敛准则，然后对式(15)的提取窗口的长度mk进行迭代，直到信息熵满足迭代收敛准则，此时提取窗口的长度mk为最优；具体为：
[0097]
基于信息熵h(sk)的迭代收敛准则如式(16)所示：
[0098][0099]
式(16)中的sk、p(sk)计算方式分别如式(17)、式(18)所示：
[0100][0101][0102]
其中，sk表示所采集到的semg信号sk的在当前的提取窗口的长度mk下的平均值，p(sk)表示当前具有mk阶自由度的卡方分布概率密度函数χ2(
·
)；
[0103]
将提取窗口的长度mk从一个设定的起始值开始累加进行迭代，然后在每次迭代完成时，判断信息熵h(sk)是否继续在增大；当信息熵h(sk)不再增大时，判定已经获得最优的提取窗口的长度mk，并迭代终止；当信息熵h(sk)还在增大时，则继续迭代；
[0104]
s23、在步骤s22获得最优的提取窗口的长度mk后，将mk代入步骤s21的式(1)对semg幅值进行自适应提取，然后输出所提取的四个通道的semg幅值；
[0105]
s3、对步骤s2自适应幅值提取后四个通道的semg幅值分别进行自适应平滑，提高semg幅值的信噪比，实现自适应的semg幅值估计；过程包括：
[0106]
采用分形维数衡量所提取semg幅值包络的跳动程度，建立平滑系数关于分形维数的指数衰减函数关系并采用粒子群算法获得最优的平滑关系；
[0107]
s31、采用分形维数评估步骤s2所提取的semg幅值包络的跳动程度；分形维数越大意味着semg幅值包络跳动越剧烈，其相应的平滑系数越小，分形维数d的计算如式(19)所示：
[0108][0109]
其中，n(δ)、n(2δ)分别如式(20)、式(21)所示：
[0110][0111]
[0112]
其中，n(δ)、n(2δ)分别表示用宽度为δ、2δ的网格覆盖semg幅值包络时所需的网格数，l表示在设定的采样周期内所采集的semg信号的样本量，ωi表示在设定的采样周期内的第i时刻的semg幅值包络；
[0113]
s32、在步骤s31获得分形维数d后，构建指数衰减函数ε与分形维数d的关系，根据粒子群优化算法获取指数衰减函数ε中最优的衰减因子a，然后采用包含最优的衰减因子a的指数衰减函数ε对步骤s2所提取的semg幅值包络进行自适应平滑；具体为：
[0114]
指数衰减函数ε与分形维数d关系如式(22)所示：
[0115]
ε＝e-ad
ꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0116]
其中，a表示衰减因子；
[0117]
采用粒子群优化算法找到全局最优的衰减因子a，本实施例优选将衰减因子a的优化范围限制为[2,6]；衰减因子a对应的目标函数，设定为操作员只在肩关节转动(即肌肉仅维持关节转动而不输出其他力)的情况下，操作员的肩关节的转角θ相对于前三角肌与后三角肌两者semg幅值融合后的融合包络ωs的相关系数最大，具体计算方式如式(23)所示：
[0118][0119]
其中，cov(ωs,θ)表示融合包络ωs与肩关节转角θ的协方差；σ
θ
分别表示融合包络ωs、肩关节转角θ各自的标准差；粒子群优化算法在获得式(23)第一行的最小值后，此时第二行的限制条件中相应的衰减因子a取值为最优值；
[0120]
将衰减因子a的最优值代入式(23)得到最优的指数衰减函数ε，然后对semg幅值包络进行平滑，具体如式(24)所示：
[0121][0122]
s33、在对semg幅值包络完成平滑后，输出相应平滑后的四个通道的semg幅值；
[0123]
s4、分别构建融合神经网络、估计神经网络，采用神经网络学习的方式，确定四个通道的semg幅值与臂力的映射关系，实现对臂力的估计，融合神经网络与估计神经网络相互连接；过程包括：
[0124]
s41、构建融合神经网络把四个通道的semg幅值转换为对应的融合特征，用于解决在臂力估计中的延时、四个通道semg幅值的融合、时序关系的捕捉这三个方面难以相互兼顾的瓶颈问题；
[0125]
融合神经网络包括依次连接的延时补偿模块、通道融合模块、时序捕捉模块；融合神经网络以平滑后四个通道的semg幅值作为输入，将其输入延时补偿模块，然后依次经过通道融合模块、时序捕捉模块的处理后，输出具有低延时、强时序关系的融合特征，具体构建为：
[0126]
在平滑后四个通道的semg幅值上，设定处理窗口的长度，融合神经网络只对位于处理窗口内的semg幅值进行处理；
[0127]
延时补偿模块以处理窗口内四个通道的semg幅值作为输入，输出为处理窗口内经过延时补偿的四个通道的semg幅值；由于四个通道的semg幅值经过幅值估计的过程会产生时滞效应，而在实时臂力估计过程中无法通过向前移动时间轴的方式消除延时，因此延时
补偿模块采用的是间接实现延时补偿的方式，间接实现延时补偿的原理为，通过分别对四个通道的semg幅值加上或减去一个延时偏差；延时偏差的大小、正负均分别与处理窗口内semg幅值的时间特性，如幅值斜率、幅值方向变化等有关；延时补偿模块中构建有一个一维卷积网络，用于分析每个通道semg幅值的时间特性，一维卷积网络利用自身的自学习能力，建立每个通道semg时间特性与延时偏差的映射关系，从而间接实现延时补偿；
[0128]
通道融合模块以处理窗口内经过延时补偿后的四个通道的semg幅值作为输入；通道融合模块内构建有一个一维卷积网络，一维卷积网络用于分析处理窗口内各个时刻四个通道的semg幅值之间的激活程度，利用自身的自学习能力确定出每个通道的semg幅值对应的融合权重系数，从而将四个通道的semg幅值进行融合，获得相应的融合特征；
[0129]
时序捕捉模块以处理窗口内经过延时补偿、通道融合后的融合特征作为输入，用于分析处理窗口内融合特征之间的时序关系，从而输出具有时序关系的融合特征；时序捕捉是通过分析当前时刻的融合特征与过去时刻的长短时记忆成分之间的相似关系，确定出当前时刻融合特征对长短时记忆更新的贡献程度，再完成长短时记忆成分的更新，从而实现时序关系的捕捉；时序捕捉模块由一个一维卷积网络和一个简化的长短时记忆更新模型组成；一维卷积网络用来分析当前时刻融合特征与过去时刻长短时记忆成分之间的相似关系，并确定出当前时刻融合特征对长短时记忆更新的贡献程度；简化的长短时记忆更新模型用于实现长短时记忆成分的更新，从而实现时序关系的捕捉；
[0130]
一维卷积网络以当前时刻的融合特征和过去时刻长短时记忆成分为输入，通过分析当前时刻的融合特征与过去时刻长短时记忆成分之间的相似关系，输出长短时记忆更新的权重系数，权重系数包括权重α、权重β；
[0131]
简化的长短时记忆更新模型如式(25)所示：
[0132][0133]
其中，
⊙
表示点乘操作，c
t-1
、h
t-1
分别表示上一时刻的长时记忆、短时记忆；权重α用于选择当前时刻的融合特征x
t
的重要部分，并控制对过去时刻短时记忆成分h
t-1
的遗忘程度，然后获得中间记忆成分m；权重β一方面用于平衡过去时刻长时记忆成分c
t-1
与中间记忆成分m，实现当前时刻短时记忆成分h
t
的更新，另一方面用于将中间记忆成分中的重要记忆加入到当前时刻的长时记忆中，实现当前时刻长时记忆c
t
的更新；
[0134]
s42、在步骤s41获得融合特征及其时序关系后，构建估计神经网络把融合特征映射到操作员的臂力上；
[0135]
估计神经网络以步骤s41获得的低延时、强时序关系的融合特征作为输入，其结构采用一个长短时记忆网络与注意力机制网络相结合的神经网络，用于建立融合特征与臂力的映射关系，具体构建为：
[0136]
长短时记忆网络以融合神经网络输出的融合特征作为输入，输出具有强化时序关系的融合特征；长短时记忆网络使用输入门i、遗忘门f、输出门o来捕捉输入融合特征x
″
t
的时序关系，在时刻t遗忘门、输入门、输出门的输出f
t
、i
t
、o
t
的计算方式分别如式(26)、式(27)、式(28)：
[0137]ft
＝σ(w
xf
x
″
t
w
hfh′
t-1
bf)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0138]it
＝σ(w
xi
x
″
t
w
hih′
t-1
bi)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(27)
[0139]ot
＝σ(w
xo
x
″
t
w
hoh′
t-1
bo)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0140]
其中，σ为非线性激活函数sigmoid，h
′
t-1
为第t-1时刻的长短时记忆网络单元的隐状态；w
xf
、w
hf
、w
xi
、w
hi
、w
xo
、w
ho
分别为可学习的权重矩阵，bf、bi、bo为偏差项；
[0141]
经过三个门的处理后，t时刻长短时记忆网络的隐藏状态h
′
t
计算公式从下列三个式子的推导而来：
[0142][0143][0144]h′
t
＝tanh(c
′
t
)
⊙ot
ꢀꢀꢀꢀꢀꢀꢀ
(31)
[0145]
其中，x
″
t
表示输入的融合特征，表示中间变量，c
′
t
是长短时记忆网络单元状态，代表长时记忆成分，w
xc
、w
hc
是可学习的权重矩阵，bc是偏差项，f
t
、i
t
、o
t
分别为遗忘门、输入门、输出门各自的输出；
[0146]
经过以上公式(32)、(33)、(34)，获得了具有强化时序关系的融合特征h
′
t
；
[0147]
注意力机制网络以具有强化时序关系的融合特征h
′
t
作为输入，其输出为估计出的臂力，注意力机制网络利用式(35)、(36)评估融合特征的重要程度，并计算出融合特征的重要性系数ζi：
[0148]
si＝tanh(wah
′i ba)
ꢀꢀꢀꢀꢀꢀ
(35)
[0149][0150]
其中，h
′i表示第i时刻具有强化时序关系的融合特征，wa,w
α
为可学习的权重矩阵，ba为偏差项，si表示中间变量，上标t表示si的转置；
[0151]
注意力机制网络按照式(37)、(38)计算获得估计的臂力：
[0152][0153][0154]
其中，h
′i表示第i时刻具有强化时序关系的融合特征，ζi表示重要性系数，x
″′
表示经过重要性系数校正后的融合特征，f(
·
)表示非线性激活函数；wf表示可学习的权重矩阵，bf表示偏差项，表示经过注意力机制网络估计出的臂力；
[0155]
s43、对所构建的融合神经网络和估计神经网络，以一种端到端的方式进行训练，通过计算最小化估计的臂力与真实臂力之间的误差，分别实现融合神经网络与估计神经网络的参数学习；将学习完成的融合神经网络与估计神经网络，从而完成处理窗口内四个通道的semg幅值到臂力的映射关系的运算；融合神经网络与估计神经网络的结构如表1所示：
[0156]
表1融合神经网络与估计神经网络的网络结构
[0157][0158]
s5、在步骤s4将semg幅值映射为臂力后，工控机根据映射关系，通过协作控制将估计出的臂力转化为协作机械臂的协作动作，让实时控制系统发送控制指令到控制柜，由控制柜给到协作机械臂信号执行相应的协作动作；协作动作为协作机械臂相应配合操作员预设动作的机械运动；协作控制的过程包括：
[0159]
同时采用pd速度控制和分段速度控制两种方式计算位移量，分别如式(39)、式(40)所示：
[0160][0161][0162]
其中，f
x
、fy分别为臂力的水平力、垂直力，表示水平力的导数，k
p
、kd分别为pd控制律的比例调节参数、微分调节参数，δx、δy分别为机器人在水平方向、垂直方向的位移；
[0163]
协作机械臂根据式(36)、式(37)的结果，执行相应的协作动作，从而实现相应的人机协作过程。
[0164]
本实施例的基于动态臂力估计的人机协作方法与现有技术相比，其有益效果在于：
[0165]
通过基于迭代学习的自适应幅值提取与基于分形维数的自适应平滑相结合的方式，实现了基于信号自身特性的特征的标准化提取，保证了提取的规范化、稳定性；兼顾臂力估计过程中的延时、多通道融合、时序关系捕捉的难点，利用融合神经网络和估计神经网
络提高了动态臂力估计的精度，利用长短时记忆网络与注意力机制实现从semg幅值到动态臂力的映射；建立了pd速度控制和分段速度控制的协作控制方法，将所估计的动态臂力转化为机器人的协作行为，具有延时小、臂力估计精度高的优点。
[0166]
在本实施例的替换实施方式中，步骤s5中的协作控制还可以采用恒力控制的方式；恒力控制的过程如下：
[0167]
将步骤s4得到的臂力记为fd，将协作机械臂末端的六维力传感器采集到实际的臂力记为f
t
，两者误差e如式(41)所示：
[0168]
e＝f
d-f
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(41)
[0169]
根据式(42)计算补偿量δf：
[0170][0171]
其中，k
p
、kd分别表示pd控制律的比例调节参数、微分调节参数，为误差e的一阶导数；
[0172]
根据补偿量δf执行式(43)：
[0173][0174]
其中，m表示惯性系数，b表示阻尼系数，k表示刚度系数，δx(t)、其中，m表示惯性系数，b表示阻尼系数，k表示刚度系数，δx(t)、分别表示位移补偿量、位移补偿量的一阶导数、位移补偿量的二阶导数；
[0175]
仅考虑刚度项，简化后协作机械臂的位置补偿如式(44)所示：
[0176][0177]
协作机械臂根据所得到的位置补偿执行协作行为，从而实现人机协作。
[0178]
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。