基于动态系统稳定估计器的机器人变刚度轨迹规划

谢啸，张涵，汤自林，高霄，肖晓晖

(武汉大学动力与机械学院，湖北武汉，430072)

机器人作为未来制造业提高生产效率、降低生产成本以及提升产品质量的重要工具，在近年来发展迅速。国内外对于机器人技能学习与控制方面的研究也与日俱增[1]。传统机器人需要使用者根据实际要求进行编程，对于使用者要求较高且任务泛化性差，限制了其应用范围。示教学习/模仿学习(learning from demonstration/imitation learning)的机器人编程方式极大地降低了技术难度，即使示教者对于机器人没有深刻认识，也能完成编程过程[2]。

许多模仿学习方法都可以实现学习并还原示教运动轨迹，其中常见的有动态运动基元DMP(dynamic movement primitives)[3-4]、概率运动基元ProMP(probabilistic movement primitives)[5-6]和GMM/GMR[7-11]等。这些算法通过采集示教运动轨迹数据进行建模，但会出现稳定性差、泛化能力以及抗干扰能力较弱等问题。其中，基于DMP 的机器人模仿学习方法仅需1次示教便能以微分方程的形式对运动轨迹进行建模，并将运动模型区分为线性部分与非线性部分，以达到复现相似度与全局稳定性俱佳的效果[12]。但DMP 采用单变量编码，其在将复杂的动态运动过程进行高效精确编码的同时，也丢失了各自由度之间的信息，导致其稳定性较差。

为了克服DMP 的不足之处，KHANSARIZADEH 等[13]提出了一种基于动态系统稳定估计器(SEDS)的方法。该法首先采用高斯混合模型GMM和高斯混合回归GMR将示教轨迹建模成动态系统模型；然后，建立动态系统全局稳定的约束方程；最后，求解包含约束的非线性优化问题，得到最优参数以完成对动态系统的建模。这种方法能很好地将从示教运动学习得到的动态系统模型轨迹平滑收敛到目标点，并且在未被示教的状态空间中也能生成相似的轨迹至目标点，具有良好的稳定性和泛化能力。但是，基于SEDS的模仿学习方法只能得到轨迹信息，并未考虑到在实际抓取和放置过程中人的阻抗特性。许金鹏等[14]引入多个规划点来校正物体的中间位置，以提高放置精度，但同时也带来了机械臂运动不自然、不连贯等问题。另外，考虑到人机交互中的安全性，机械臂常使用阻抗控制，其中控制器的刚度不仅会影响机器人到达目标点的位置精度，也会改变其放置物体过程中的能量消耗，是一个不可忽视的参数。

针对刚度的学习问题，YANG等[15]结合了基于EMG(electromyographic)的变阻抗控制和DMP模型的优点，实现了人-机器人技能转移。首先，将人的上肢肌电图信号提取出来用以获得其肌肉刚度特征，以此作为阻抗控制器的可变增益；然后利用DMP 同时对轨迹和刚度信息进行分析；最终根据实际情况生成更符合人类特性的机器人运动过程。

本文基于物体放置场景中的位置精度以及能量消耗这2 项要求，首先利用SEDS 对示教运动进行建模，生成参考轨迹，然后结合GMM/GMR 学习到的人放置物体的刚度信息，实现机械臂的阻抗控制，使得其在放置时的位置精度较高、抗干扰能力较强，整个过程中的能量消耗减小。

1 变刚度的SEDS轨迹规划

变刚度的SEDS 轨迹规划的具体流程如图1 所示。图1中，示教过程中的轨迹信息和肌电信号由相应传感器采集并存储，而后分别对采集到的数据进行对齐处理。轨迹信息采用SEDS模仿学习方法，先对动态系统建模，确定约束条件后求得动态系统的最优参数并建立基于状态反馈的模型，从而得到新任务的轨迹。刚度学习则是先找到人上肢末端肌电信号和刚度的映射关系，将预处理后的肌电信号映射到刚度空间，通过GMM拟合得到运动过程中刚度平均值的概率分布，再通过GMR 进行回归，最终得到机器人运动时的刚度变化曲线。轨迹和刚度协同作用可以使机器人更好地模仿人类并执行放置任务，提高物体放置的位置精度，减少能量消耗。

在机器人模仿学习过程中，示教者带动机器人完成任务，采集轨迹信息并建立如下数据集：

式中：ξ为状态变量，用于表征机械臂末端的位置信息；n为1 个示教任务的示教次数，n=1，2，…，N；t为1个示教任务内的时间戳，t=0，1，2，…，Tn。对于示教轨迹数据，一般使用一阶动态系统来建模，其一阶自洽常微分方程可表示为

式中：f:Rd→Rd，为1 个具有单平衡点的非线性连续可微函数；Rd为d维向量张成的空间；R为实数集；ε为高斯噪声项，其均值为0，包含了传感器测量中的不确定性和误差。

为了获取运动过程中伴随人手臂的刚度信息，实验中采用MYO肌电手环采集肌电信号，然后将8个通道的肌电信号制作成如下数据集：

式中：η为状态变量，用于表征8 个通道的肌电信息。

将ξ和η这2 个数据集作为输入，使机器人在模仿学习过程中同时具备使轨迹和刚度变化的能力。在实际场景中，对于运动轨迹和刚度变化示教，示教者在拖动机械臂完成1次放置物体过程中的运动轨迹和手臂上的肌电信号均被采集用于模仿学习。

2 轨迹学习

对于点对点示教学习问题，采用动态系统稳定估计器SEDS对机器人运动进行非线性动态系统建模，采用GMM对控制策略建模，并通过稳定性约束确保全局渐近稳定性[16]。

2.1 基于统计方法的高斯混合模型

通过基于统计方法的高斯混合模型(GMM)建立对于动态系统的概率估计模型，得到动态系统的具体表达式。对于1组示教次数为N的示教轨迹数据集GMM 的概率密度函数表达式如下：

式中：P(ξt,n,;θ)为高斯过程概率密度函数；d为数据维度；k为高斯分布的数量，k=1，2，…，K；α(k)为观测数据属于第k个高斯子模型的概率，且有为第k个高斯分量的待确定参数，且有θ={θ1,θ2,…,θk}，k=1，2，…，K；φ(ξt,n,∣θk)为第k个子模型的高斯分布密度函数；μk为该密度函数的均值向量；Mk为协方差矩阵。

由条件期望求得整个高斯混合模型中ξ的总期望值为

式中：i=1，2，…，K。

通过换元对式(7)进行简化：

得到示教运动的动态系统模型的具体表达式如下：

2.2 动态系统全局稳定的充分条件

为了保证模型的泛化能力，将其从不同的初始点到达同一目标点的学习任务转化为具有全局收敛性的学习任务，此时，需要考虑模型的稳定性问题。SEDS 中的稳定性理论基于二次能量函数，由Lyapunov 稳定性定理推导出保证该系统全局渐近稳定的充分条件，并采用二次规划求解[17]。

根据Lyapunov 稳定性理论，其在点ξ*处达到全局渐近稳定的充分条件是存在一个连续可微的Lyapunov函数V(ξ):Rd→R，且满足如下关系式：

构造Lyapunov函数：

求得保证动态系统全局稳定的充分条件：

式中：k= 1, 2,…,K；≺指矩阵的负定性。

2.3 动态系统参数学习

为了得到具有全局渐近稳定性的动态系统，需要确定计算其未知参数的方法。动态系统未知参数θk={μk,Mk}，k=1，2，…，K；全部参数θ={θ1,θ2,…,θk}。通过将动态系统参数学习的过程转化为求解非线性优化问题，在保证模型具有全局渐近稳定性的条件下，求解优化问题来计算θ的最优值。对似然函数求对数，然后建立如下求解模型：

约束条件为：

式中：J(θ)为非线性规划目标函数；为示教数据的总轨迹点数。

借助基于拟牛顿法的连续二次规划方法[18]可以解决以上优化问题，得到具有全局渐近稳定性的动态系统的最优参数θ，实现对于动态系统的完整描述。

经过SEDS 模仿学习得到的仿真结果如图2 所示。从图2可以看出：新的轨迹反映了示教轨迹的趋势，并且能从各个位置收敛到目标点。

3 刚度学习

3.1 刚度映射

人体手臂的关节刚度与肌肉收缩活动、人体手臂姿势、肌肉拉伸反射等因素有关。本文采用一种经过验证的、简化的人体手臂关节刚度估计模型[19-20]对示教者放置物体过程中的肌电信号进行刚度映射，并参考彭涛[21]提出的方法，计算得到肌电信号和刚度之间的映射矩阵为

具体的映射对应关系如图3 所示。图3(a)所示为8个通道的肌电信号值，图3(b)所示为通过映射矩阵求出的x，y和z方向的刚度变化曲线，可以看出3 个方向的刚度Kx，Ky和Kz具有相同的变化趋势，且x和y方向的刚度较为接近，z方向刚度较小，也符合人手臂的实际情况。

3.2 GMM拟合概率分布

在高斯混合模型中，混合高斯分量个数K的选取对数据拟合精度具有重要的影响。一般采用贝叶斯信息准则BIC(Bayesian information criterion)估计K的最佳值：

式中：L为似然函数；m为模型参数的个数；e为样本数量；SBIC为贝叶斯准则中用来表征信息量的参数。

在x方向上，不同K对应的SBIC如图4所示。由图4可见：当3＜K≤5时，SBIC下降趋势变缓；当K＞5时，SBIC呈明显的上升趋势，不符合要求。结合实验测试结果，本文选取K= 5。在y和z方向上，SBIC的变化趋势与其在x方向上的一致，故均取K= 5。

GMM 的未知参数包括μk和Mk，最大期望算法EM(expectation-maximum)可以用来计算参数的解。

EM算法通过不断建立最大似然函数的下界(E步骤)并优化下界(M 步骤)，通过迭代来估计函数的最大值。首先需要初始化模型参数，然后逐步进行E步骤和M步骤的迭代计算。

1)E 步骤，求最大似然函数期望值，以Kx为例进行说明。由当前参数计算x方向上第j个刚度=1，2，…，D)来自第k个子模型的可能性γjk，

2)M 步骤，求最大似然函数极大值。更新下一轮迭代的模型参数：

由EMG 信号与刚度的对应关系，通过转换矩阵可以将示教者放置物体过程中采集到的肌电信号映射成刚度变化曲线，继而通过GMM拟合得到图5(a)，(c)和(e)。

3.3 GMR回归生成刚度曲线

GMR 方法首先需要计算数据集中样本之间的联合概率分布f～θ(μk,Mk)，再根据需要预测的f*的先验概率分布f*～θ(μk*,Mk*)来计算f*的后验概率分布。与其他回归算法相比，GMR 不需要直接建立回归函数，计算速度快且输入和输出都可以是多维的。

对所得的GMM 模型采用GMR 回归拟合得到的最大概率曲线如图5(b)，(d)和(f)所示。由图5可见：0～2 s 为夹取物体阶段，刚度较稳定；2～10 s为放置物体阶段，为保证到达目标点的位置精度，刚度逐渐增大；在10～12 s 时，为了减小物体与平面接触的冲击力，刚度迅速减小。

4 实验

为了验证变刚度的SEDS算法的有效性，本文设计基于模仿学习的机械臂执行物体放置的实验。首先，加入变刚度阻抗控制，并将机械臂的位置精度和能量消耗与定刚度条件下的相应参数进行对比，之后加入位置扰动以验证SEDS 轨迹的收敛性。

4.1 实验平台搭建

物体放置实验平台如图6 所示。机械臂选用Franka Emika 公司的Panda 协作机械臂，为基于力矩控制的7 自由度机械臂。操作员将MYO 肌电手环佩戴在大臂上，手环内圈的感应器紧贴大臂肌肉，拖动机械臂末端示教物体放置过程，采集肌电信号，采用GMR 回归生成该过程的刚度曲线，作为机械臂阻抗控制的参数。以2 个质量为500 g的砝码作为机械臂末端额外负载，用于能量消耗对比实验。将世界坐标系原点固定在实验台桌面上，建立如图6所示的右手笛卡尔坐标系。

根据轨迹和刚度变化的学习结果以及新任务给出的终点坐标，生成新的轨迹与相应的刚度，并将其离散为各点的期望位置坐标与x，y和z这3个方向上的刚度，以预设的时间间隔(本实验为0.1 s)为周期逐一发送给机械臂。

4.2 阻抗控制

一般地，串联机械臂的刚体动力学模型可描述为

式中：q，和∈Rn，分别为关节位置、速度和加速度矢量；G(q)∈Rn×n，为质量矩阵；c(q,)∈Rn，为科里奥利力(Coriolis)矩阵；g(q)∈Rn，为重力项；J(q)为正运动学的雅克比矩阵；fext∈R6，为笛卡尔空间的接触外力/力矩；τ∈Rn，为电机控制力矩。

本实验机械臂的控制算法采用阻抗控制器，笛卡尔空间阻抗控制器的控制律为

由式(22)可知机械臂与外界的交互关系可用弹簧-阻尼系统模型表征，若无外力，则轨迹将收敛至期望轨迹。本文考虑参考姿态和当前姿态的最短距离(四元数的测地线距离)，该方法相较于由欧拉角直接求差所得的姿态计算精度更高。

阻抗控制框架如图7所示。采集多组人工示教数据，其中轨迹信息用于SEDS轨迹学习，肌电信号用于刚度学习。轨迹规划阶段基于当前位置反馈更新参考信号，结合刚度信息作为阻抗控制器参数，发送给机械臂，以完成期望的操作任务。

4.3 实验结果分析

4.3.1 物体放置实验

由于世界坐标系中的z方向为物体放置方向，位置精度要求相对较低，故本文主要研究x和y方向上阻抗控制器刚度参数对物体放置点位置精度的影响。由3.3节可知2个方向刚度变化趋势一致，故以y方向为例进行分析。图8(a)所示为当轨迹相同时，定刚度和变刚度控制下机械臂在y方向上到达目标终点的位置误差对比。在定刚度控制条件下，以100 N/m为增幅，设定刚度梯度，模型刚度变化范围为100～1 000 N/m。而变刚度模型则根据示教数据拟合出的刚度变化曲线进行离散处理，将离散后的刚度作为阻抗控制器参数进行实验。在每种刚度下进行20次实验，误差取平均值。从图8可以看出在变刚度条件下机械臂末端到达目标点时的位置误差为0.007 6 m，介于700 N/m和800 N/m定刚度下所得位置误差之间，相较于定刚度100 N/m下的位置误差(0.034 7 m)，减小了78.1%。

对机械臂末端施加不同的负载(无负载，500 g，1 000 g，1 500 g)，对比定刚度和变刚度阻抗控制时机械臂所产生的能耗，实验结果表明其变化趋势一致，故以500 g 负载条件为例进行分析，每种刚度条件下进行20 次实验，能耗取平均值，结果如图8(b)所示。由图8(b)可以看出：变刚度条件下机械臂能耗为0.036 J，介于定刚度600 N/m 和700 N/m 下机械臂的能耗之间，相较于定刚度下(1 000 N/m)的0.042 9 J，能耗减小16.1%。

综合上述2 个实验结果，假设存在1 个定刚度，若该条件下的位置误差比变刚度条件下的小，其刚度应大于700 N/m；若该条件下的能耗比变刚度条件下的小，其刚度应小于700 N/m。这两者相矛盾，故不存在这样一种定刚度。由此可以看出相较于定刚度运动，变刚度运动在位置精度和能量消耗之间可取得较好的平衡。

4.3.2 抗干扰实验

在原实验的基础上，对机械臂添加位置上的干扰以测试模型的鲁棒性。在无干扰和引入干扰条件下，机械臂在x，y和z这3个方向上的轨迹曲线如图9 所示。在t≈2.5 s 时(图9 所示红色线段)，操作员沿y轴正方向推动机械臂，以改变其末端位置，约1.5 s后停止推动，如图9(b)所示。从图9中灰色区域可以看出：机械臂末端y方向坐标值发生了较大变化，x和z方向坐标值无明显变化；当外力撤去后，在基于SEDS 的运动规划算法的作用下，机械臂末端重新收敛到期望终点。

5 结论

1)加入刚度控制后，机器人到达目标点的位置精度较高，能量消耗较少，表明机械臂学习到了人放置物体过程中的轨迹与阻抗变化趋势。

2)受到外界干扰后，机械臂会根据控制器发出的期望坐标位置调整自身运动，最终仍能收敛到目标点，具有较强的抗干扰能力。