人和机器人协作可以提高生产效率，改善产品质量，是机器人领域的研究热点之一^[1-3]. 人机协作主要分为接触式和非接触式2种，协作搬运属于接触式人机协作. 实现人机协作搬运常用的2种方式分别为基于传统控制的策略和基于学习的策略.

基于传统控制的策略主要采用阻抗/导纳控制，将机械臂末端等效为质量-弹簧-阻尼系统. Bussy等^[4]结合导纳控制和有限状态机实现人和机器人角色的切换，并基于触觉信息验证人机协作搬运角色的静态分配和动态分配策略，证明动态角色分配策略效果更好，但协作搬运效果依赖于对运动基元的划分. Agravante 等^[5-6]结合视觉和触觉信息确保被搬运的物体水平抬升. 这类基于传统控制的策略可以实现自由搬运任务（搬运前并不知道搬运终点，或者只知道大概的搬运终点区域，或者在搬运过程中须临时变更终点位置），且搬运的柔顺性较好，但是搬运终点的位置须人为控制，适用于对搬运终点位置精度要求不高的场合. 除此之外，这类方法还须针对机器人运动相关的任务模型来设计控制器参数，在学习新任务或者添加新约束时须重新设计控制器.

基于学习的策略常用方法为模仿学习^[7]，使用机器学习的方法建立状态和动作的概率模型，可以快速实现任务的复现或泛化. Vogt等^[8]使用交互式网格(interactive meshes, IMs)表示两人协作示教的时间和空间关系，并通过隐含马尔可夫模型(hidden Markov model, HMM)将每个IMs组合起来. 这种方法可以复现复杂的连续任务，但没有涉及接触力信息，且动作滞后. Gu等^[9]提出结合模仿学习和强化学习的方法，模仿学习用于实现仿人机器人伸手抓取桌子的过程，强化学习则用于保证桌子水平抬升，但是整个过程只涉及到桌子的上下运动一个自由度. 除此之外，还有高斯混合模型结合高斯混合回归(Gaussian mixture model + Gaussian mixture regression, GMM+GMR)^[10]，这种方法把问题转化为对模型的训练和轨迹的生成，不需要复杂编程，但针对的是特定搬运任务（搬运之前已经明确搬运终点，任务是定的. 终点可以通过视觉或其他方式定位），泛化能力不够；同GMM类似，动态运动基元(dynamic movement primitives, DMP)^[11]也不需要复杂编程，并能泛化轨迹到不同起点和终点，但有较多参数须仔细调节（包括速度阈值和权重因子），而且也是针对特定搬运任务的；任务参数化的高斯混合模型（task-parameterized Gaussian mixture model，TP-GMM）模型^[12-15]可以针对不同起点和终点生成轨迹，但是不能实现自由搬运任务，在增加机械臂末端柔性后易运动到期望区域外，且终点位置精度得不到保证. HMM模型^[16]可以实现对力的简单预测，但在学习力和位置信号阶段中，分割算法须根据信号维度手动调整，且通过接触力预测并生成连续轨迹受到限制；除了模仿学习外，Dumora等^[17]采集力矩信息并使用朴素贝叶斯分类器(naive Bayes classifier)预测人的行为，实现了未知任务中机器人对人的协作，但这种方法在批量学习中须提供整个数据集.

为了实现机械臂快速适应不同的协作搬运场景，并保证搬运过程的柔顺性和一定的搬运终点位置精度，提出基于模仿学习的变刚度协作搬运控制策略：使用TP-GMM模型学习搬运示教数据并生成轨迹；以此为参考轨迹，结合导纳控制，根据轨迹点的高斯分布协方差矩阵设计导纳控制的刚度系数，实现柔性搬运的变刚度控制，并增加基于交互力阈值的搬运任务切换，以提高协作搬运的灵活性. 主要创新点如下：1）修正TP-GMM模型起点和终点附近概率分布，并依据模型设计变刚度系数的导纳控制，保证搬运终点的位置精度以及机械臂末端在期望区域运动；2）根据示教数据确定交互力阈值，并用于切换搬运任务，保证特定搬运任务到自由搬运任务的平稳切换，使得系统能够适应多种搬运任务.

采用基于TP-GMM的方法实现协作搬运轨迹规划. TP-GMM可以根据不同任务参数生成对应的轨迹概率模型，对于抓取和搬运，设定起点和终点坐标系作为任务参数，可以生成从不同起点到不同终点的轨迹概率模型.

TP-GMM整体流程如图1所示，主要分为3个阶段：第1阶段是采集示教数据，对轨迹重采样对齐数据；第2阶段是模型训练，把示教轨迹分别变换到起点和终点坐标系，使用期望最大化(expectation maximization, EM)算法求解TP-GMM参数^[18]，得到起点坐标系和终点坐标系的GMM，并使用GMR对所得GMM回归分别得到起点和终点坐标系的GMR；第3阶段是新任务轨迹生成，把2个GMR经坐标变换到新的起点和终点坐标系，并进行对应时间序列点的高斯乘积和线性变换，最终得到起点到终点的轨迹概率模型.

协作搬运数据采集系统如图2所示，左侧为主操作者搬运物体一侧移动，物体另一侧由协助者拖动示教机械臂夹持运动，两人协作完成搬运任务示教. 采集机械臂末端位置和力，对轨迹进行重采样后所得轨迹数据集为 ${\boldsymbol{{ X}}} \in {{{\bf{R}}}^{D \times T \times P}}$. 其中， $P$为轨迹数量；维度 $D{{\rm{ = }}}4$，包括 $ t、x、y、z $， $t$为采样点的序号； $T$为一条轨迹上采样点的数量. 示教数据还包括每条轨迹起点和终点的坐标系，这里 $M{{\rm{ = }}}2$为任务参数个数，对应起点坐标系和终点坐标系 $ \left\{ {\left. {{{\boldsymbol{b}}_m},{{\boldsymbol{A}}_m}} \right\}} \right._{m = 1}^M $. 其中， ${{\boldsymbol{b}}_m}$为坐标系的原点； ${{\boldsymbol{A}}_m}$为坐标系的姿态，即旋转矩阵.

利用示教轨迹数据集进行模型训练，首先将示教轨迹分别变换到起点和终点坐标系进行TP-GMM编码，求取轨迹点的概率分布模型. TP-GMM参数包括 $\left\{ {{\pi}_i},\left\{ {\boldsymbol{\mu }}_i^{(m)},{\boldsymbol{\varSigma }}_i^{(m)}\right\}_{m = 1}^M\right\}_{i = 1}^K$. 其中， $ {{\pi}_i} $为第 $i$个高斯分布混合系数， $ {\boldsymbol{\mu} }_{i}^{(m)}、{\boldsymbol{\varSigma} }_{i}^{(m)} $分别为第 $i$个高斯分布在第 $m$个坐标系下的均值和协方差矩阵. 参数 $K$根据贝叶斯准则(Bayesian information criterion, BIC)确定，计算公式为

(1) $ {S_{{{\rm{BIC}}}}} = - 2\ln\; L{{\rm{ + }}}y\ln\; z\; . $

式中： $L$为似然函数， $y$为模型参数个数， $z$为样本数量. 如图3所示，在 $K = {{\rm{4}}}$之后 ${S_{{{\rm{BIC}}}}}$下降趋势变缓，结合实际测试最后选定 $K = 3$. 模型其余参数使用EM算法确定.

EM算法分2步迭代，不断更新均值和协方差矩阵直到对数似然函数收敛或者达到最大迭代次数，本研究取对数似然函数终止迭代值为 $1 \times {10^{ - 5}}$，最大迭代次数为100. 迭代计算公式如下：

1）E-step.

(2) $ \gamma (i,k) = \frac{{{{\text{π}}_k}\displaystyle \prod\nolimits_{m = 1}^M {\mathcal{N}\left( {{\boldsymbol{x}}_i^{{{\rm{(}}}m{{\rm{)}}}}\Big|{\boldsymbol{\mu }}_k^{{{\rm{(}}}m{{\rm{)}}}},{\boldsymbol{\varSigma }}_k^{{{\rm{(}}}m{{\rm{)}}}}} \right)} }}{{\displaystyle \sum\nolimits_{j = 1}^K {{{\text{π}}_j}\displaystyle \prod\nolimits_{m = 1}^M {\mathcal{N}\left( {{\boldsymbol{x}}_i^{{{\rm{(}}}m{{\rm{)}}}}\Big|{\boldsymbol{\mu }}_j^{{{\rm{(}}}m{{\rm{)}}}},{\boldsymbol{\varSigma }}_j^{{{\rm{(}}}m{{\rm{)}}}}} \right)} } }}\; .$

2）M-step.

(3) $ {\text{π}}_{k}=\frac{{\displaystyle {\sum }_{i=1}^{N}\gamma (i,k)}}{N}{\rm{\hspace{0.17em}}}{\rm{，}}{\boldsymbol{\mu} }_{k}^{{\rm{(}}m{\rm{)}}}=\frac{{\displaystyle {\sum }_{i=1}^{N}\gamma (i,k)}{\boldsymbol{x}}_{i}^{{\rm{(}}m{\rm{)}}}{\rm{\hspace{0.17em}}}}{{\displaystyle {\sum }_{i=1}^{N}\gamma (i,k)}}{\rm{\hspace{0.17em}}},$

(4) $ {\boldsymbol{\varSigma }}_k^{{{\rm{(}}}m{{\rm{)}}}} = \frac{{\displaystyle \sum\nolimits_{i = 1}^N {\gamma (i,k)} \left( {{\boldsymbol{x}}_i^{{{\rm{(}}}m{{\rm{)}}}} - {\boldsymbol{\mu }}_k^{{{\rm{(}}}m{{\rm{)}}}}} \right){{\left( {{\boldsymbol{x}}_i^{{{\rm{(}}}m{{\rm{)}}}} - {\boldsymbol{\mu }}_k^{{{\rm{(}}}m{{\rm{)}}}}} \right)}^{{\rm{T}}}}}}{{\displaystyle \sum\nolimits_{i = 1}^N {\gamma (i,k)} }}\; . $

式中： ${{\boldsymbol{x}}_i}$为第 $i$个示教数据， $N$为示教数据点总数.

训练得到不同坐标系下GMM模型分别如图4(a)、(b)所示. 图中，实线为示教数据，椭球为高斯分布1-sigma区域.

将所得GMM模型分别进行GMR回归^[18]，以时间作为输入，轨迹三维坐标作为输出，求对应条件概率的概率分布为

(5) $ \mathcal{P}\left( {{\boldsymbol{\xi }}_t^{{\rm{O}}}|\xi _t^{{\rm{I}}}} \right) \sim \sum\limits_{i = 1}^K {{h_i}} \left( {\xi _t^{{\rm{I}}}} \right)\mathcal{N}\left( {{\boldsymbol{\hat \mu }}_i^{{\rm{O}}}\left( {\xi _t^{{\rm{I}}}} \right),{\boldsymbol{\hat \varSigma }}_i^{{\rm{O}}}} \right)\; \cdot $

式中： $ \xi _t^{{\rm{I}}} $、 $ {\boldsymbol{\xi }}_t^{{\rm{O}}} $、 $ \mu _i^{{\rm{I}}} $、 $ {\boldsymbol{\mu }}_i^{{\rm{O}}} $、 $ \varSigma _i^{{\rm{I}}} $、 $ {\boldsymbol{\varSigma }}_i^{{\rm{O}}} $、 $ {\boldsymbol{\varSigma }}_i^{{{\rm{IO}}}} $、 $ {\boldsymbol{\varSigma }}_i^{{{\rm{OI}}}} $分别为 ${{\boldsymbol{\xi }}_t}$、 ${{\boldsymbol{\mu }}_i}$、 ${{\boldsymbol{\varSigma }}_i}$切分成输入、输出而成， ${\boldsymbol{\xi}}_t $为时间和对应的轨道位置的概率分布的均值构成的四维向量. 表达式如下：

(6) $ {{\boldsymbol{\xi }}_t} = \left[ {\begin{array}{*{20}{l}} {\xi _t^{{\rm{I}}}} \\ {{\boldsymbol{\xi }}_t^{{\rm{O}}}} \end{array}} \right],\;\;{{\boldsymbol{\mu }}_i} = \left[ {\begin{array}{*{20}{c}} {\mu _i^{{\rm{I}}}} \\ {{\boldsymbol{\mu }}_i^{{\rm{O}}}} \end{array}} \right],\;\;{{\boldsymbol{\varSigma }}_i} = \left[ {\begin{array}{*{20}{c}} {\varSigma _i^{{\rm{I}}}}&{{\boldsymbol{\varSigma }}_i^{{{\rm{IO}}}}} \\ {{\boldsymbol{\varSigma }}_i^{{{\rm{OI}}}}}&{{\boldsymbol{\varSigma }}_i^{{\rm{O}}}} \end{array}} \right]\; , $

(7) $ {\boldsymbol{\hat \mu }}_i^{{\rm{O}}}\left( {\xi _t^{{\rm{I}}}} \right) = {\boldsymbol{\mu }}_i^{{\rm{O}}} + {\boldsymbol{\varSigma }}_i^{{{\rm{OI}}}}{(\varSigma _i^{{\rm{I}}})^{ - 1}}\left( {\xi _t^{{\rm{I}}} - \mu _i^{{\rm{I}}}} \right)\; , $

(8) $ {\boldsymbol{\hat \varSigma }}_i^{{\rm{O}}} = {\boldsymbol{\varSigma }}_i^{{\rm{O}}} - {\boldsymbol{\varSigma }}_i^{{\rm{O}}}{(\varSigma _i^{{\rm{I}}})^{ - 1}}{\boldsymbol{\varSigma }}_i^{{{\rm{IO}}}}\; , $

(9) $ {h_i}\left( {\xi _t^{{\rm{I}}}} \right) = \frac{{{{\text{π}}_i}\mathcal{N}\left( {\xi _t^{{\rm{I}}}|\mu _i^{{\rm{I}}},\varSigma _i^{{\rm{I}}}} \right)}}{{\displaystyle \sum\nolimits_{k{{\rm{ = }}}1}^K {{{\text{π}}_k}} \mathcal{N}\left( {\xi _t^{\rm{I}}|\mu _k^{{\rm{I}}},\varSigma _k^{{\rm{I}}}} \right)}}\; . $

得到在起点和终点坐标系下轨迹概率分布的GMR模型，如图5(a)、(b)所示. 图中，实线为轨迹概率模型均值，浅色区域为轨迹点对应高斯分布的椭球相互重叠形成.

在模型训练好后可以针对不同任务参数生成对应的轨迹概率模型. 把训练得到的起点和终点坐标系下GMR模型分别变换到新的起点和终点坐标系，然后进行对应序列的高斯分布乘积，计算公式为

(10) $ \mathcal{N}\left( {{{\boldsymbol{\mu }}_t},{{\boldsymbol{\varSigma }}_t}} \right) \propto \prod\limits_{m = 1}^M \mathcal{N} \left( {{{\boldsymbol{A}}_m}{\boldsymbol{\mu }}_t^{(m)} + {{\boldsymbol{b}}_m},{{\boldsymbol{A}}_m}\Sigma _t^{(m)}{\boldsymbol{A}}_m^{{\rm{T}}}} \right)\; . $

式中： $ \{ {{\boldsymbol{b}}_m},{{\boldsymbol{A}}_m}\} _{m = 1}^M $为新的起点和终点坐标系； $\left\{ {\boldsymbol{\mu }}_t^{(m)}, $ $ {\boldsymbol{\varSigma }}_t^{(m)}\right\} _{m = 1}^M$为训练得到的起点和终点坐标系下 $t$时刻轨迹点的均值和协方差矩阵； $ {{\boldsymbol{\mu }}_t} $、 $ {{\boldsymbol{\varSigma }}_t} $为新生成轨迹点的均值和协方差矩阵，表达式分别为

(11) $ {{\boldsymbol{\varSigma }}_t} = {\left( {\sum\limits_{m = 1}^M {{{\left( {{{\boldsymbol{A}}_m}{\boldsymbol{\varSigma }}_t^{(m)}{\boldsymbol{A}}_m^{{\rm{T}}}} \right)}^{ - 1}}} } \right)^{ - 1}}\; ,$

(12) $ {{\boldsymbol{\mu }}_t} = {{\boldsymbol{\varSigma }}_t}\sum\limits_{m = 1}^M {{{\left( {{{\boldsymbol{A}}_m}{\boldsymbol{\varSigma }}_t^{(m)}{\boldsymbol{A}}_m^{{\rm{T}}}} \right)}^{ - 1}}} \left( {{{\boldsymbol{A}}_m}{\boldsymbol{\mu }}_t^{(m)} + {{\boldsymbol{b}}_m}} \right)\; .$

对训练所得模型进行轨迹生成测试，得到不同终点生成的轨迹均值，如图6所示. 图中只绘制了轨迹的概率分布中均值位置. 由于生成的轨迹在起点和终点位置有较小偏差，通过平移缩放对齐起点和终点，得到最终的轨迹概率模型.

修正TP-GMM生成的初始概率模型并结合变刚度导纳控制可以获得需要的精度和柔顺性的组合. 针对须提高柔顺性的情况，可以适当放大对应点附近的协方差矩阵. 针对须提高位置精度的情况，可以缩小对应点附近的协方差矩阵. 针对提高搬运起点和终点的位置精度，使用缩小函数缩小起点和终点附近的协方差矩阵，协方差矩阵修正为

(13) $ {{\boldsymbol{\varSigma }}_{{\rm{n}}}}{{\rm{ = }}}{\boldsymbol{Q}} \cdot S(x) \cdot {\boldsymbol{\varLambda }}{{\boldsymbol{Q}}^{{\rm{T}}}}\; .$

式中： $x$为任务完成度； ${\boldsymbol{Q}}、{\boldsymbol{\varLambda }}$分别为 ${{\boldsymbol{\varSigma }}_{\rm{n}}} $对角分布得到的正交矩阵和对角矩阵，即 ${{\boldsymbol{\varSigma }}_{\rm{n}}}{\rm{ = }}{\boldsymbol{Q }}{\boldsymbol{\varLambda}}{{\boldsymbol{Q}}^{\rm{T}}} $； $ S(x) $为缩小函数，其形式和参数根据任务对搬运柔顺性和终点位置精确性的需求来选取，这里选择二次函数，具体为

(14) $ S(x) = \left\{ \begin{gathered} - 99{(x - 0.1)^2} + 1,\;\;\;\;0 \leqslant x < 0.1; \hfill \\ 1,\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;0.1 \leqslant x \leqslant 0.9;\; \hfill \\ - 99{(x - 0.9)^2} + 1,\;\;\;\;0.9 < x \leqslant 1.0. \hfill \end{gathered} \right. $

TP-GMM生成的原始轨迹概率模型和经修正后的轨迹概率模型分别如图7(a)、(b)所示. 图中，实线为轨迹概率模型均值. 通过对比可以看出，修正后模型在起点和终点附近的区域变小（可以通过起点和浅色区域的距离来判断），结合变刚度阻抗控制提高相应区域位置精度.

以TP-GMM模型生成的轨迹为参考轨迹，并结合导纳控制模型^[19]表示机械臂末端在笛卡尔坐标系下的协作搬运交互模型，公式为

(15) $ {(\Delta {\boldsymbol{f}}_t^{\rm e} {{\rm{ - }}}{\boldsymbol{K}}_t^{{\rm{v}}}\Delta {{\boldsymbol{\dot x}}_t})_{{{L}}}} = {\boldsymbol{K}}_t^{{\rm{p}}}{\left( {\Delta {{\boldsymbol{x}}_t}} \right)_{{{L}}}}\; . $

式中： $ \Delta {\dot{\boldsymbol{x}}}_{t}、\Delta {\boldsymbol{x}}_{t} $分别为机械臂末端实际运动的速度、位置与参考轨迹速度、位置的差值； $ {\boldsymbol{K}}_{t}^{{\rm{v}}}、{\boldsymbol{K}}_{t}^{{\rm{p}}} $分别为阻尼和刚度矩阵； $ \Delta {\boldsymbol{f}}_t^{{\rm{e}}} $为机械臂末端力矩传感器数据经过重力补偿后的力，即干扰力；下标L表示在全局坐标系G下以高斯分布均值为原点、主轴方向为坐标轴建立的局部坐标系L. 坐标系关系如图8所示，为了方便观察坐标系关系，绘制的是二维高斯分布，椭圆为等概率密度线.

参考轨迹上每个点对应轨迹概率分布中的一个高斯分布，依据高斯分布确定每个点不同方向具有的不同刚度. 如图8所示，不同方向刚度与椭圆(三维高斯分布是椭球)主轴长度成反比，得到L坐标系下交互模型为

(16) $ {(\Delta {\boldsymbol{f}}_t^{{\rm{e}}}{{\rm{ - }}}{\boldsymbol{K}}_t^{{\rm{v}}}\Delta {{\boldsymbol{\dot x}}_t})_{{{L}}}} = {\sqrt {\boldsymbol{\varLambda }} ^{{{\rm{ - }}}1}}{k_{{\rm{p}}}}{\left( {\Delta {{\boldsymbol{x}}_t}} \right)_{{{L}}}}\;. $

式中： $ {\boldsymbol{\varLambda }} $为轨迹点对应高斯分布协方差矩阵分解所得对角矩阵， ${k_{{\rm{p}}}}$为刚度矩阵因子（轨迹偏离一个标准差距离对应末端受力）. $ {\boldsymbol{\varLambda }} $满足高斯分布和对应协方差矩阵：

(17) $ \mathcal{N}({\boldsymbol{x}}|{\boldsymbol{\mu }},{\boldsymbol{\varSigma }}) = {{{{(2{\text{π}})}^{-{D}/{2}}}}}{{|{\boldsymbol{\varSigma }}{|^{-{1}/{2}}}}}\exp\; \left\{ { - \frac{1}{2}{{({\boldsymbol{x}} - {\boldsymbol{\mu }})}^{{\rm{T}}}}{{\boldsymbol{\varSigma }}^{ - 1}}({\boldsymbol{x}} - {\boldsymbol{\mu }})} \right\}\; , $

(18) $ {\boldsymbol{\varSigma }}{{\rm{ = }}}{\boldsymbol{Q\varLambda }}{{\boldsymbol{Q}}^{{\rm{T}}}}{{\rm{ = }}}{\boldsymbol{Q}}\left[ \begin{gathered} {\lambda _1}\;0\;\;\;0 \hfill \\ 0\;\;{\lambda _2}\;0 \hfill \\ 0\;\;0\;\;{\lambda _3} \hfill \end{gathered} \right]{{\boldsymbol{Q}}^{{\rm{T}}}}\;. $

依据式(17)、(18)可以得到坐标系G到坐标系L的变换矩阵 $ {}_G^L{\boldsymbol{R}} = {{\boldsymbol{Q}}^{{\rm{T}}}} $，因此，在坐标系G下交互模型为

(19) $ {(\Delta {\boldsymbol{f}}_t^{{\rm{e}}}{{\rm{ - }}}{\boldsymbol{K}}_t^{{\rm{v}}}\Delta {{\boldsymbol{ x}}_t})_{{{G}}}} = {k_{{\rm{p}}}}{\boldsymbol{Q}}{\sqrt {\boldsymbol{\varLambda }} ^{{{\rm{ - }}}1}}{{\boldsymbol{Q}}^{{\rm{T}}}}{\left( {\Delta {{\boldsymbol{x}}_t}} \right)_{{{G}}}}\; . $

式中：下标G表示全局坐标系G. 则在坐标系G下刚度矩阵为

(20) $ {({\boldsymbol{K}}_t^{{\rm{p}}}{{\rm{)}}}_{{{G}}}}{{\rm{ = }}}{k_{{\rm{p}}}} {\boldsymbol{Q}}{\sqrt {\boldsymbol{\varLambda }} ^{ - 1}}{{\boldsymbol{Q}}^{{\rm{T}}}}\; \cdot $

由式(19)、(20)可以得到坐标系 $G$下系统传递函数为

(21) $ {\boldsymbol{P}}(s){{\rm{ = }}}\frac{{{\boldsymbol{X}}(s)}}{{{\boldsymbol{F}}(s)}} = \frac{1}{{{{\left({\boldsymbol{K}}_t^{{\rm{v}}} s + {\boldsymbol{K}}_t^{{\rm{p}}}\right)}_{{{G}}}}}}\; \cdot $

为了保证机械臂末端具有一定柔性，同时距离参考轨迹不超过3个标准差，设定刚度矩阵因子随着距参考轨迹点距离增大而增大，本研究中刚度矩阵因子 ${k_{{\rm{p}}}}$为指数函数，系数为参考轨迹中心刚度，根据示教数据估计，由交互力与高斯分布个数比值求均值所得，表达式为

(22) $ {k_{{\rm{p}}}} = 4\exp\; ({n^2/8})\; \cdot $

式中： $n$为机械臂末端点 $(x,y,z)$到参考点 $ {\boldsymbol{\mu }}_t^{} $的距离与这2点构成的直线和协方差椭球面交点到参考点的距离的比值.

(23) $ \frac{{{{(x - {\mu _{tx}})}^2}}}{{{n^2}{\lambda _1}}}{{\rm{ + }}}\frac{{{{(y - {\mu _{ty}})}^2}}}{{{n^2}{\lambda _2}}}{{\rm{ + }}}\frac{{{{(z - {\mu _{tz}})}^2}}}{{{n^2}{\lambda _3}}} = 1\; \cdot $

式中： $ {\;\mu }_{tx}、{\mu }_{ty}、{\mu }_{tz} $分别为 $ {\boldsymbol{\mu }}_t^{} $对应的 $ x、y、z $分量. 结合图7(b)，在高斯分布3-sigma区域中，刚度系数在均值 $ {\boldsymbol{\;\mu }}_t^{} $处最小，随着距离增加而增大，在灰色边界处达到最大.

综上，设计特定搬运任务控制器如图9所示. 图中， $ {{\boldsymbol{x}}_{{\rm{r}}}} $为TP-GMM模型生成的参考轨迹上的点， $ {{\boldsymbol{F}}_0} $为物体的重力， ${{\boldsymbol{F}}_{{{\rm{ext}}}}}$为末端力矩传感器采集的数据. 据此在Matlab/Simulink中搭建控制模型并生成C++代码^[20]，集成到ROS机器人操作系统(robot operating system)中，实现对机械臂的变刚度控制.

搬运任务主要包括特定搬运任务和自由搬运任务. 以TP-GMM模型生成的轨迹为参考轨迹，结合导纳控制的方法具有一定的抗干扰能力，但无法适应自由搬运任务.

采用基于阈值的搬运任务切换，在正常搬运情况下交互内力在一定范围内，当交互力大于阈值时系统则由特定搬运任务切换为自由搬运任务，从而可以完成自由搬运任务. 自由搬运任务采用PI控制器，输入机械臂末端受力，经低通滤波器和控制器，输出机械臂末端速度.

阈值的选取对于系统任务切换性能至关重要. 根据中心极限定理^[21]，随机变量交互力服从高斯分布. 对示教数据中交互力的模长计算均值和标准差，得到交互力的概率分布，力阈值初始值选取在6个标准差处，后续可以随搬运任务的执行不断更新，力阈值会越来越准确. 根据示教的交互力得到的概率分布如图10所示，得到初始力阈值为29.89 N. 图中，P为概率， $||F|| $为力模长.

协作搬运系统如图2所示. 搬运物体重量为0.5、1.0、1.5、2.0、2.5 kg，交互模型中刚度系数分别为本研究变刚度方法和现有变刚度方法所得刚度系数，以及恒定刚度系数，阻尼系数为 $\sqrt {2{k_{\rm{p}}}}\; $. 基于ROS平台搭建协作搬运控制系统，选择UR5e作为协作机械臂，并使用Vicon Motion Systems（光学运动捕捉系统）对搬运对象定位.

搬运整体流程如图11所示，主要分为2个阶段. 1）抓取阶段：场景如图12(a)所示，定位点放置于手和被搬运物体上，通过动作捕捉系统获取目标和人手的位姿，当检测到人手不断接近物体时，控制机械臂按照TP-GMM模型生成的轨迹运动到物体上方实现抓取，抓取阶段不需要机械臂末端柔性. 2）协作搬运阶段：场景如图12(b)所示，人和机械臂协同抬升物体2 cm并停留0.5 s，进行重力补偿（对已知重量的物体不用停留）. 随后以模型生成的轨迹为参考轨迹，在一定交互力下沿着期望轨迹的方向运动.

现有恒定刚度方法^[13]中刚度系数在整条轨迹上恒定，变刚度方法^[12]中刚度系数在不同高斯分布椭球内不同. 本研究变刚度方法刚度系数在高斯分布椭球内部由中心向外增加，在参考轨迹处刚度相同. 针对这3种方法进行交互实验对比，同一种情况下分别重复6次实验. 由于机械臂末端有六维力/力矩传感器对物体进行重力补偿，并且重量变化不大，不同重量结果类似，这里以及后面的实验展示的物体质量都是2 kg.

在变刚度交互模型下分别施加 $x、y、z$正负方向约25 N交互力，如图13所示. 图中，椭圆区域为高斯分布3个标准差区域（由于多个椭球叠加遮挡影响轨迹数据，这里只绘制了路径中的几个椭球，实际区域和图7(b)一致），实线为TP-GMM模型生成机械臂末端参考轨迹，其余线条分别为在 $x、y、z$方向施加力对应的机械臂末端实际轨迹. 在恒定刚度交互模型下（轨迹概率模型没有经过修正），分别对刚度系数为100、1200 N/m的交互模型以及现有变刚度方法的 $x$方向施加力进行实验. 如图14所示为根据示教数据估计得到的刚度系数在整条轨迹上的变化情况. 图中，K₀为刚度，R为任务完成度. 采集到的机械臂末端轨迹如图15所示，如表1所示为不同方法对应的轨迹平均偏差和终点位置偏差. 图中，轨迹起始重合是由于开始搬运一段时间后施加了力.

1）在恒刚度方法中，当刚度系数较大时，轨迹偏差减小，但交互性不好，且无法保证终点的位置精度，如图15所示，刚度系数较小导致机械臂末端运动超过3个标准差，运动到期望区域外，也无法保证终点的位置精度. 如表1所示，恒定刚度系数对应终点位置偏差均较大，当刚度系数较大时终点位置偏差较小. 2）现有变刚度方法平衡了一部分搬运的柔顺性和终点位置精度，但是终点的位置精度不够高，这与估计出的刚度有关系. 如图14所示，估计的刚度较小，在图15中干扰力下易跑出范围，须人施加反方向力保持轨迹，抗干扰能力差. 3）所提出的变刚度方法可以保证交互的柔顺性并提高终点位置精度，同时限制机械臂末端运动轨迹在参考轨迹3个标准差范围内. 如表1所示，变刚度方法终点位置偏差更小，平均刚度（轨迹上点对应刚度的平均值）在较大和较小刚度系数对应的平均刚度之间. 这是因为变刚度控制策略允许搬运过程中平均刚度较小，而当搬运目标靠近终点时，修正后的模型轨迹协方差矩阵迅速变小，交互刚度迅速增大以提高搬运终点的位置精度. 由图13可以看出，变刚度系数对应轨迹在参考轨迹的3个标准差范围之内，且终点收敛到目标点. 这是因为刚度系数在快到3个标准差边界时刚度迅速增加，限制了运动范围.

针对现有变刚度方法以及本研究变刚度方法，分别在 $x、y、z$方向施加冲击干扰得到机械臂末端轨迹如图16所示. 图中，星型点划线为本研究变刚度方法的轨迹，虚线为现有变刚度方法的轨迹. 可以看出，本研究变刚度方法中机械臂在一定干扰力的情况下仍能完成协作搬运任务；现有变刚度方法受到干扰之后偏离期望位置较大，无法完成任务，这是由于现有变刚度方法估计得到的刚度较小，抵抗干扰的能力较弱.

在特定搬运任务运行一段时间后施加逐渐增大的交互力直到超过阈值，切换成自由搬运任务，自由选择终点. 如图17所示为搬运任务切换对应机械臂末端轨迹. 图中，星号标记处为临时决定的搬运终点，点划线为本研究变刚度方法的轨迹，虚线为现有变刚度方法的轨迹. 可以看出，利用本研究的变刚度方法可以在想要的位置切换为自由搬运任务，增加了搬运的灵活性；利用现有变刚度方法则没办法直接往目标方向走，因为在中间阶段刚度系数较大，在刚度变小之后才能往目标方向走，绕了一圈，而且 $z$方向偏离目标位置较大.

本研究变刚度方法中机械臂关节速度如图18所示. 图中， $\omega $为角速度. 第1阶段为特定搬运任务，第2阶段（灰色框内）为自由搬运任务. 搬运任务的切换造成关节速度均值从0.1 rad/s增加到0.3 rad/s，变化较大，这是因为切换的位置刚度较大，而自由搬运任务刚度较小，从而切换搬运任务关节速度变化较大.

（1）基于TP-GMM模型生成的轨迹概率模型提出变刚度交互模型. 基于示教数据学习协作搬运交互轨迹概率模型，并基于导纳控制设计和轨迹方差设计变刚度，实现机械臂末端柔性交互，并且末端实际位置在参考轨迹的3个标准差之内，保证搬运终点的位置精度.

（2）考虑搬运任务的切换，以交互力阈值作为特定搬运任务和自由搬运任务切换的依据，提高协作搬运的灵活性.

（3）通过实验对比本研究变刚度交互和现有交互方法下协作搬运结果，验证了本研究变刚度交互的有效性. 本研究方法在保证搬运柔顺性的同时将特定搬运任务的终点位置精度提高到1.9 mm.

本研究只考虑了机械臂末端的三维位置，后续将考虑进行协作搬运的位置和姿态的同时规划. 对于切换搬运任务速度变化较大的问题，后续考虑优化切换搬运任务的策略.